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

Add spaces around comparison operators in engine source files.

Browse Source 
PiperOrigin-RevId: 535989348
Change-Id: I883f7e82351299933c49b35a31842b5d8d6aea04
This commit is contained in:
Yuval Tassa
2023-05-28 05:01:55 -07:00
committed by Copybara-Service
parent d40c395917
commit 455b1cd2e2
29 changed files with 2224 additions and 2219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+104 -104
View File
@@ -50,14 +50,14 @@ void mj_kinematics(const mjModel* m, mjData* d) {
mj_normalizeQuat(m, d->qpos);
// normalize mocap quaternions
for (int i=0; i<m->nmocap; i++) {
for (int i=0; i < m->nmocap; i++) {
mju_normalize4(d->mocap_quat+4*i);
}
// compute global cartesian positions and orientations of all bodies
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
// free joint
if (m->body_jntnum[i]==1 && m->jnt_type[m->body_jntadr[i]]==mjJNT_FREE) {
if (m->body_jntnum[i] == 1 && m->jnt_type[m->body_jntadr[i]] == mjJNT_FREE) {
// get addresses
int jid = m->body_jntadr[i];
int qadr = m->jnt_qposadr[jid];
@@ -81,7 +81,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
int pid = m->body_parentid[i];
// get body pos and quat: from model or mocap
if (m->body_mocapid[i]>=0) {
if (m->body_mocapid[i] >= 0) {
bodypos = d->mocap_pos + 3*m->body_mocapid[i];
bodyquat = d->mocap_quat + 4*m->body_mocapid[i];
} else {
@@ -95,7 +95,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
mju_mulQuat(quat, d->xquat+4*pid, bodyquat);
// accumulate joints, compute pos and quat for this body
for (int j=0; j<m->body_jntnum[i]; j++) {
for (int j=0; j < m->body_jntnum[i]; j++) {
// get joint id, qpos address, joint type
int jid = m->body_jntadr[i] + j;
int qadr = m->jnt_qposadr[jid];
@@ -117,7 +117,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
case mjJNT_BALL:
case mjJNT_HINGE:
// compute local quaternion rotation (qloc)
if (jtype==mjJNT_BALL) {
if (jtype == mjJNT_BALL) {
mju_copy4(qloc, d->qpos+qadr);
} else {
mju_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]);
@@ -152,21 +152,21 @@ void mj_kinematics(const mjModel* m, mjData* d) {
}
// compute/copy Cartesian positions and orientations of body inertial frames
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
mj_local2Global(d, d->xipos+3*i, d->ximat+9*i,
m->body_ipos+3*i, m->body_iquat+4*i,
i, m->body_sameframe[i]);
}
// compute/copy Cartesian positions and orientations of geoms
for (int i=0; i<m->ngeom; i++) {
for (int i=0; i < m->ngeom; i++) {
mj_local2Global(d, d->geom_xpos+3*i, d->geom_xmat+9*i,
m->geom_pos+3*i, m->geom_quat+4*i,
m->geom_bodyid[i], m->geom_sameframe[i]);
}
// compute/copy Cartesian positions and orientations of sites
for (int i=0; i<m->nsite; i++) {
for (int i=0; i < m->nsite; i++) {
mj_local2Global(d, d->site_xpos+3*i, d->site_xmat+9*i,
m->site_pos+3*i, m->site_quat+4*i,
m->site_bodyid[i], m->site_sameframe[i]);
@@ -186,7 +186,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
mju_zero(d->subtree_com, m->nbody*3);
// backwards pass over bodies: compute subtree_com and mass_subtree
for (int i=m->nbody-1; i>=0; i--) {
for (int i=m->nbody-1; i >= 0; i--) {
// add local info
mju_addToScl3(d->subtree_com+3*i, d->xipos+3*i, m->body_mass[i]);
mass_subtree[i] += m->body_mass[i];
@@ -199,7 +199,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
}
// compute local com
if (mass_subtree[i]<mjMINVAL) {
if (mass_subtree[i] < mjMINVAL) {
mju_copy3(d->subtree_com+3*i, d->xipos+3*i);
} else {
mju_scl3(d->subtree_com+3*i, d->subtree_com+3*i,
@@ -208,14 +208,14 @@ void mj_comPos(const mjModel* m, mjData* d) {
}
// map inertias to frame centered at subtree_com
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
mju_sub3(offset, d->xipos+3*i, d->subtree_com+3*m->body_rootid[i]);
mju_inertCom(d->cinert+10*i, m->body_inertia+3*i, d->ximat+9*i,
offset, m->body_mass[i]);
}
// map motion dofs to global frame centered at subtree_com
for (int j=0; j<m->njnt; j++) {
for (int j=0; j < m->njnt; j++) {
// get dof address, body index
int da = 6*m->jnt_dofadr[j];
int bi = m->jnt_bodyid[j];
@@ -229,7 +229,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
case mjJNT_FREE:
// translation components: x, y, z in global frame
mju_zero(d->cdof+da, 18);
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
d->cdof[da+3+7*i] = 1;
}
@@ -238,7 +238,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
mjFALLTHROUGH;
case mjJNT_BALL:
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
// I_3 rotation in child frame (assume no subsequent rotations)
axis[0] = d->xmat[9*bi+i+0];
axis[1] = d->xmat[9*bi+i+3];
@@ -268,7 +268,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
mjtNum pos[3], matT[9];
// compute Cartesian positions and orientations of cameras
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
// default processing for fixed mode
mj_local2Global(d, d->cam_xpos+3*i, d->cam_xmat+9*i,
m->cam_pos+3*i, m->cam_quat+4*i, m->cam_bodyid[i], 0);
@@ -285,7 +285,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
mju_copy(d->cam_xmat+9*i, m->cam_mat0+9*i, 9);
// position: track camera body
if (m->cam_mode[i]==mjCAMLIGHT_TRACK) {
if (m->cam_mode[i] == mjCAMLIGHT_TRACK) {
mju_add3(d->cam_xpos+3*i, d->xpos+3*id, m->cam_pos0+3*i);
}
@@ -298,9 +298,9 @@ void mj_camlight(const mjModel* m, mjData* d) {
case mjCAMLIGHT_TARGETBODY:
case mjCAMLIGHT_TARGETBODYCOM:
// only if target body is specified
if (id1>=0) {
if (id1 >= 0) {
// get position to look at
if (m->cam_mode[i]==mjCAMLIGHT_TARGETBODY) {
if (m->cam_mode[i] == mjCAMLIGHT_TARGETBODY) {
mju_copy3(pos, d->xpos+3*id1);
} else {
mju_copy3(pos, d->subtree_com+3*id1);
@@ -328,7 +328,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
}
// compute Cartesian positions and directions of lights
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
// default processing for fixed mode
mj_local2Global(d, d->light_xpos+3*i, 0, m->light_pos+3*i, 0, m->light_bodyid[i], 0);
mju_rotVecQuat(d->light_xdir+3*i, m->light_dir+3*i, d->xquat+4*m->light_bodyid[i]);
@@ -345,7 +345,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
mju_copy3(d->light_xdir+3*i, m->light_dir0+3*i);
// position: track light body
if (m->light_mode[i]==mjCAMLIGHT_TRACK) {
if (m->light_mode[i] == mjCAMLIGHT_TRACK) {
mju_add3(d->light_xpos+3*i, d->xpos+3*id, m->light_pos0+3*i);
}
@@ -358,9 +358,9 @@ void mj_camlight(const mjModel* m, mjData* d) {
case mjCAMLIGHT_TARGETBODY:
case mjCAMLIGHT_TARGETBODYCOM:
// only if target body is specified
if (id1>=0) {
if (id1 >= 0) {
// get position to look at
if (m->light_mode[i]==mjCAMLIGHT_TARGETBODY) {
if (m->light_mode[i] == mjCAMLIGHT_TARGETBODY) {
mju_copy3(pos, d->xpos+3*id1);
} else {
mju_copy3(pos, d->subtree_com+3*id1);
@@ -416,7 +416,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
// loop over tendons
for (int i=0; i<nten; i++) {
for (int i=0; i < nten; i++) {
// initialize tendon path
adr = m->tendon_adr[i];
d->ten_wrapadr[i] = wcnt;
@@ -424,13 +424,13 @@ void mj_tendon(const mjModel* m, mjData* d) {
// sparse Jacobian row init
if (issparse) {
rowadr[i] = (i>0 ? rowadr[i-1] + rownnz[i-1] : 0);
rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0);
}
// process joint tendon
if (m->wrap_type[adr]==mjWRAP_JOINT) {
if (m->wrap_type[adr] == mjWRAP_JOINT) {
// process all defined joints
for (int j=0; j<m->tendon_num[i]; j++) {
for (int j=0; j < m->tendon_num[i]; j++) {
// get joint id
int k = m->wrap_objid[adr+j];
@@ -455,11 +455,11 @@ void mj_tendon(const mjModel* m, mjData* d) {
int x, *list = colind+rowadr[i];
mjtNum y, *listy = J+rowadr[i];
for (int k=1; k<rownnz[i]; k++) {
for (int k=1; k < rownnz[i]; k++) {
x = list[k];
y = listy[k];
int j = k-1;
while (j>=0 && list[j]>x) {
while (j >= 0 && list[j] > x) {
list[j+1] = list[j];
listy[j+1] = listy[j];
j--;
@@ -475,7 +475,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
// process spatial tendon
divisor = 1;
int j = 0;
while (j<m->tendon_num[i]-1) {
while (j < m->tendon_num[i]-1) {
// get 1st and 2nd object
tp0 = m->wrap_type[adr+j];
id0 = m->wrap_objid[adr+j];
@@ -483,9 +483,9 @@ void mj_tendon(const mjModel* m, mjData* d) {
id1 = m->wrap_objid[adr+j+1];
// pulley
if (tp0==mjWRAP_PULLEY || tp1==mjWRAP_PULLEY) {
if (tp0 == mjWRAP_PULLEY || tp1 == mjWRAP_PULLEY) {
// get divisor, insert obj=-2
if (tp0==mjWRAP_PULLEY) {
if (tp0 == mjWRAP_PULLEY) {
divisor = m->wrap_prm[adr+j];
mju_zero3(d->wrap_xpos+wcnt*3);
d->wrap_obj[wcnt] = -2;
@@ -504,7 +504,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
wbody[0] = m->site_bodyid[id0];
// second object is geom: process site-geom-site
if (tp1==mjWRAP_SPHERE || tp1==mjWRAP_CYLINDER) {
if (tp1 == mjWRAP_SPHERE || tp1 == mjWRAP_CYLINDER) {
// reassign, get 2nd site info
tpw = tp1;
idw = id1;
@@ -513,19 +513,19 @@ void mj_tendon(const mjModel* m, mjData* d) {
// do wrapping, possibly get 2 extra points (wlen>=0)
sideid = mju_round(m->wrap_prm[adr+j+1]);
if (sideid<-1 || sideid>=m->nsite) {
if (sideid < -1 || sideid >= m->nsite) {
mju_error("Invalid sideid %d in wrap_prm", sideid); // SHOULD NOT OCCUR
}
wlen = mju_wrap(wpnt+3, d->site_xpos+3*id0, d->site_xpos+3*id1,
d->geom_xpos+3*idw, d->geom_xmat+9*idw, m->geom_size+3*idw, tpw,
(sideid>=0 ? d->site_xpos+3*sideid : 0));
(sideid >= 0 ? d->site_xpos+3*sideid : 0));
} else {
tpw = mjWRAP_NONE;
}
// complete sequence, accumulate lengths
if (wlen<0) {
if (wlen < 0) {
mju_copy3(wpnt+3, d->site_xpos+3*id1);
wbody[1] = m->site_bodyid[id1];
L[i] += mju_dist3(wpnt, wpnt+3)/divisor;
@@ -537,8 +537,8 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
// accumulate moments if consequtive points are in different bodies
for (int k=0; k<(wlen<0 ? 1 : 3); k++) {
if (wbody[k]!=wbody[k+1]) {
for (int k=0; k < (wlen < 0 ? 1 : 3); k++) {
if (wbody[k] != wbody[k+1]) {
// get 3D position difference, normalize
mju_sub3(dif, wpnt+3*k+3, wpnt+3*k);
mju_normalize3(dif);
@@ -581,19 +581,19 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
// assign to wrap
mju_copy(d->wrap_xpos+wcnt*3, wpnt, (wlen<0 ? 3:9));
mju_copy(d->wrap_xpos+wcnt*3, wpnt, (wlen < 0 ? 3:9));
d->wrap_obj[wcnt] = -1;
if (wlen>=0) {
if (wlen >= 0) {
d->wrap_obj[wcnt+1] = d->wrap_obj[wcnt+2] = idw;
}
d->ten_wrapnum[i] += (wlen<0 ? 1:3);
wcnt += (wlen<0 ? 1:3);
d->ten_wrapnum[i] += (wlen < 0 ? 1:3);
wcnt += (wlen < 0 ? 1:3);
// advance
j += (tpw!=mjWRAP_NONE ? 2 : 1);
j += (tpw != mjWRAP_NONE ? 2 : 1);
// assign last site before pulley or tendon end
if (j==m->tendon_num[i]-1 || m->wrap_type[adr+j+1]==mjWRAP_PULLEY) {
if (j == m->tendon_num[i]-1 || m->wrap_type[adr+j+1] == mjWRAP_PULLEY) {
mju_copy3(d->wrap_xpos+wcnt*3, d->site_xpos+3*id1);
d->wrap_obj[wcnt] = -1;
d->ten_wrapnum[i]++;
@@ -634,7 +634,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
int *chain;
// compute lengths and moments
for (int i=0; i<nu; i++) {
for (int i=0; i < nu; i++) {
// extract info
id = m->actuator_trnid[2*i];
idslider = m->actuator_trnid[2*i+1]; // for slider-crank only
@@ -645,13 +645,13 @@ void mj_transmission(const mjModel* m, mjData* d) {
case mjTRN_JOINT: // joint
case mjTRN_JOINTINPARENT: // joint, force in parent frame
// slide and hinge joint: scalar gear
if (m->jnt_type[id]==mjJNT_SLIDE || m->jnt_type[id]==mjJNT_HINGE) {
if (m->jnt_type[id] == mjJNT_SLIDE || m->jnt_type[id] == mjJNT_HINGE) {
length[i] = d->qpos[m->jnt_qposadr[id]]*gear[0];
moment[i*nv + m->jnt_dofadr[id]] = gear[0];
}
// ball joint: 3D wrench gear
else if (m->jnt_type[id]==mjJNT_BALL) {
else if (m->jnt_type[id] == mjJNT_BALL) {
// j: qpos start address
int j = m->jnt_qposadr[id];
@@ -659,7 +659,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_quat2Vel(axis, d->qpos+j, 1);
// gearAxis: rotate to parent frame if necessary
if (m->actuator_trntype[i]==mjTRN_JOINT) {
if (m->actuator_trntype[i] == mjTRN_JOINT) {
mju_copy3(gearAxis, gear);
} else {
mju_negQuat(quat, d->qpos+j);
@@ -691,7 +691,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_quat2Vel(axis, d->qpos+j+3, 1);
// gearAxis: rotate to world frame if necessary
if (m->actuator_trntype[i]==mjTRN_JOINT) {
if (m->actuator_trntype[i] == mjTRN_JOINT) {
mju_copy3(gearAxis, gear+3);
} else {
mju_negQuat(quat, d->qpos+j+3);
@@ -720,7 +720,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
av = mju_dot3(vec, axis);
det = av*av + rod*rod - mju_dot3(vec, vec);
ok = 1;
if (det<=0) {
if (det <= 0) {
ok = 0;
sdet = 0;
length[i] = av;
@@ -748,15 +748,15 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_subFrom(jac, jacS, 3*nv);
// apply chain rule
for (int j=0; j<nv; j++) {
for (int k=0; k<3; k++) {
for (int j=0; j < nv; j++) {
for (int k=0; k < 3; k++) {
moment[i*nv+j] += dlda[k]*jacA[k*nv+j] + dldv[k]*jac[k*nv+j];
}
}
// scale by gear ratio
length[i] *= gear[0];
for (int j = 0; j<nv; j++) {
for (int j = 0; j < nv; j++) {
moment[i*nv + j] *= gear[0];
}
break;
@@ -767,7 +767,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// moment: sparse or dense
if (mj_isSparse(m)) {
int end = d->ten_J_rowadr[id] + d->ten_J_rownnz[id];
for (int j=d->ten_J_rowadr[id]; j<end; j++) {
for (int j=d->ten_J_rowadr[id]; j < end; j++) {
moment[i*nv + d->ten_J_colind[j]] = d->ten_J[j] * gear[0];
}
} else {
@@ -878,7 +878,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// count all relevant contacts, accumulate Jacobians
int counter = 0;
for (int j=0; j<d->ncon; j++) {
for (int j=0; j < d->ncon; j++) {
const mjContact* con = d->contact+j;
int b1 = m->geom_bodyid[con->geom1];
int b2 = m->geom_bodyid[con->geom2];
@@ -893,14 +893,14 @@ void mj_transmission(const mjModel* m, mjData* d) {
counter++;
// condim 1 or elliptic cones: normal is in the first row
if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) {
if (con->dim == 1 || m->opt.cone == mjCONE_ELLIPTIC) {
efc_force[con->efc_address] = 1;
}
// pyramidal cones: average all pyramid directions
else {
int npyramid = con->dim-1; // number of frictional directions
for (int k=0; k<2*npyramid; k++) {
for (int k=0; k < 2*npyramid; k++) {
efc_force[con->efc_address+k] = 0.5/npyramid;
}
}
@@ -919,7 +919,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// accumulate in moment_exclude
if (issparse) {
for (int k=0; k<NV; k++) {
for (int k=0; k < NV; k++) {
moment_exclude[chain[k]] += jac[k];
}
} else {
@@ -963,8 +963,8 @@ void mj_crbSkip(const mjModel* m, mjData* d, int skipsimple) {
mju_copy(crb, d->cinert, 10*m->nbody);
// backward pass over bodies, accumulate composite inertias
for (int i=m->nbody-1; i>0; i--) {
if (m->body_parentid[i]>0) {
for (int i=m->nbody-1; i > 0; i--) {
if (m->body_parentid[i] > 0) {
mju_addTo(crb+10*m->body_parentid[i], crb+10*i, 10);
}
}
@@ -973,7 +973,7 @@ void mj_crbSkip(const mjModel* m, mjData* d, int skipsimple) {
mju_zero(d->qM, m->nM);
// dense backward pass over dofs
for (int i=m->nv-1; i>=0; i--) {
for (int i=m->nv-1; i >= 0; i--) {
// copy
if (skipsimple && m->dof_simplenum[i]) {
d->qM[m->dof_Madr[i]] = m->dof_M0[i];
@@ -990,7 +990,7 @@ void mj_crbSkip(const mjModel* m, mjData* d, int skipsimple) {
// sparse backward pass over ancestors
int j = i;
while (j>=0) {
while (j >= 0) {
// M(i,j) += cdof_j * crb_body(i) * cdof_i = cdof_j * tmp
d->qM[Madr_ij] += mju_dot(d->cdof+6*j, tmp, 6);
@@ -1027,12 +1027,12 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
mju_copy(qLD, M, m->nM);
// dense backward loop over dofs (regular only, simple diagonal already copied)
for (int k=nv-1; k>=0; k--) {
for (int k=nv-1; k >= 0; k--) {
// get address of M(k,k)
Madr_kk = dof_Madr[k];
// check for small/negative numbers on diagonal
if (qLD[Madr_kk]<mjMINVAL) {
if (qLD[Madr_kk] < mjMINVAL) {
mj_warning(d, mjWARN_INERTIA, k);
qLD[Madr_kk] = mjMINVAL;
}
@@ -1045,11 +1045,11 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
// sparse backward loop over ancestors of k (excluding k)
Madr_ki = Madr_kk + 1;
int i = dof_parentid[k];
while (i>=0) {
while (i >= 0) {
tmp = qLD[Madr_ki] / qLD[Madr_kk]; // tmp = M(k,i) / M(k,k)
// get number of ancestors of i (including i)
if (i<nv-1) {
if (i < nv-1) {
cnt = dof_Madr[i+1] - dof_Madr[i];
} else {
cnt = m->nM - dof_Madr[i+1];
@@ -1067,7 +1067,7 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
}
// compute 1/diag(D), 1/sqrt(diag(D))
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
mjtNum qLDi = qLD[dof_Madr[i]];
qLDiagInv[i] = 1.0/qLDi;
if (qLDiagSqrtInv) {
@@ -1096,9 +1096,9 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
int nv = m->nv;
// single vector
if (n==1) {
if (n == 1) {
// x <- inv(L') * x; skip simple, exploit sparsity of input vector
for (int i=nv-1; i>=0; i--) {
for (int i=nv-1; i >= 0; i--) {
if (!m->dof_simplenum[i] && x[i]) {
// init
int Madr_ij = dof_Madr[i]+1;
@@ -1106,7 +1106,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
// traverse ancestors backwards
// read directly from x[i] since i cannot be a parent of itself
while (j>=0) {
while (j >= 0) {
x[j] -= qLD[Madr_ij++]*x[i]; // x(j) -= L(i,j) * x(i)
// advance to parent
@@ -1116,12 +1116,12 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
}
// x <- inv(D) * x
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
x[i] *= qLDiagInv[i]; // x(i) /= L(i,i)
}
// x <- inv(L) * x; skip simple
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
if (!m->dof_simplenum[i]) {
// init
int Madr_ij = dof_Madr[i]+1;
@@ -1129,7 +1129,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
// traverse ancestors backwards
// write directly in x[i] since i cannot be a parent of itself
while (j>=0) {
while (j >= 0) {
x[i] -= qLD[Madr_ij++]*x[j]; // x(i) -= L(i,j) * x(j)
// advance to parent
@@ -1145,16 +1145,16 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
mjtNum tmp;
// x <- inv(L') * x; skip simple
for (int i=nv-1; i>=0; i--) {
for (int i=nv-1; i >= 0; i--) {
if (!m->dof_simplenum[i]) {
// init
int Madr_ij = dof_Madr[i]+1;
int j = dof_parentid[i];
// traverse ancestors backwards
while (j>=0) {
while (j >= 0) {
// process all vectors, exploit sparsity
for (offset=0; offset<n*nv; offset+=nv)
for (offset=0; offset < n*nv; offset+=nv)
if ((tmp = x[i+offset])) {
x[j+offset] -= qLD[Madr_ij]*tmp; // x(j) -= L(i,j) * x(i)
}
@@ -1167,14 +1167,14 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
}
// x <- inv(D) * x
for (int i=0; i<nv; i++) {
for (offset=0; offset<n*nv; offset+=nv) {
for (int i=0; i < nv; i++) {
for (offset=0; offset < n*nv; offset+=nv) {
x[i+offset] *= qLDiagInv[i]; // x(i) /= L(i,i)
}
}
// x <- inv(L) * x; skip simple
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
if (!m->dof_simplenum[i]) {
// init
int Madr_ij = dof_Madr[i]+1;
@@ -1182,9 +1182,9 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
// traverse ancestors backwards
tmp = x[i+offset];
while (j>=0) {
while (j >= 0) {
// process all vectors
for (offset=0; offset<n*nv; offset+=nv) {
for (offset=0; offset < n*nv; offset+=nv) {
x[i+offset] -= qLD[Madr_ij]*x[j+offset]; // x(i) -= L(i,j) * x(j)
}
@@ -1222,11 +1222,11 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n)
mju_copy(x, y, n * nv);
// loop over the n input vectors
for (int ivec=0; ivec<n; ivec++) {
for (int ivec=0; ivec < n; ivec++) {
int offset = ivec*nv;
// x <- inv(L') * x; skip simple, exploit sparsity of input vector
for (int i=nv-1; i>=0; i--) {
for (int i=nv-1; i >= 0; i--) {
mjtNum tmp;
if (!m->dof_simplenum[i] && (tmp = x[i+offset])) {
// init
@@ -1234,7 +1234,7 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n)
int j = dof_parentid[i];
// traverse ancestors backwards
while (j>=0) {
while (j >= 0) {
x[j+offset] -= qLD[Madr_ij++] * tmp; // x(j) -= L(i,j) * x(i)
// advance to parent
@@ -1244,7 +1244,7 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n)
}
// x <- sqrt(inv(D)) * x
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
x[i+offset] *= qLDiagSqrtInv[i]; // x(i) /= sqrt(L(i,i))
}
}
@@ -1262,7 +1262,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
mju_zero(d->cvel, 6);
// forward pass over bodies
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
// get body's first dof address
int bda = m->body_dofadr[i];
@@ -1270,7 +1270,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
mju_copy(cvel, d->cvel+6*m->body_parentid[i], 6);
// cvel = cvel_parent + cdof * qvel, cdofdot = cvel x cdof
for (int j=0; j<m->body_dofnum[i]; j++) {
for (int j=0; j < m->body_dofnum[i]; j++) {
// compute cvel and cdofdot
switch (m->jnt_type[m->dof_jntid[bda+j]]) {
case mjJNT_FREE:
@@ -1287,7 +1287,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
case mjJNT_BALL:
// compute all 3 cdofdots using parent velocity
for (int k=0; k<3; k++) {
for (int k=0; k < 3; k++) {
mju_crossMotion(cdofdot+6*(j+k), cvel, d->cdof+6*(bda+j+k));
}
@@ -1326,7 +1326,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
mjtNum* body_vel = mj_stackAlloc(d, 6*m->nbody);
// bodywise quantities
for (int i=0; i<m->nbody; i++) {
for (int i=0; i < m->nbody; i++) {
// compute and save body velocity
mj_objectVelocity(m, d, mjOBJ_BODY, i, body_vel+6*i, 0);
@@ -1342,7 +1342,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
}
// subtree linvel
for (int i=m->nbody-1; i>=0; i--) {
for (int i=m->nbody-1; i >= 0; i--) {
// non-world: add linear momentum to parent
if (i) {
mju_addTo3(d->subtree_linvel+3*m->body_parentid[i], d->subtree_linvel+3*i);
@@ -1354,7 +1354,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
}
// subtree angmom
for (int i=m->nbody-1; i>0; i--) {
for (int i=m->nbody-1; i > 0; i--) {
int parent = m->body_parentid[i];
// momentum wrt body i
@@ -1399,7 +1399,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
}
// forward pass over bodies: accumulate cacc, set cfrc_body
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
// get body's first dof address
int bda = m->body_dofadr[i];
@@ -1424,13 +1424,13 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
mju_zero(loc_cfrc_body, 6);
// backward pass over bodies: accumulate cfrc_body from children
for (int i=m->nbody-1; i>0; i--)
for (int i=m->nbody-1; i > 0; i--)
if (m->body_parentid[i]) {
mju_addTo(loc_cfrc_body+6*m->body_parentid[i], loc_cfrc_body+6*i, 6);
}
// result = cdof * cfrc_body
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6);
}
@@ -1453,7 +1453,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// cfrc_ext = perturb
mju_zero(d->cfrc_ext, 6*nbody);
for (int i=1; i<nbody; i++)
for (int i=1; i < nbody; i++)
if (!mju_isZero(d->xfrc_applied+6*i, 6)) {
// rearrange as torque:force
mju_copy3(cfrc, d->xfrc_applied+6*i+3);
@@ -1467,8 +1467,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// cfrc_ext += contacts
for (int i=0; i<d->ncon; i++)
if (d->contact[i].efc_address>=0) {
for (int i=0; i < d->ncon; i++)
if (d->contact[i].efc_address >= 0) {
// get contact pointer
con = d->contact+i;
@@ -1502,7 +1502,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// cfrc_ext += connect and weld constraints
int i = 0;
while (i < d->ne) {
if (d->efc_type[i]!=mjCNSTR_EQUALITY)
if (d->efc_type[i] != mjCNSTR_EQUALITY)
mju_error("Row %d of efc is not an equality constraint", i); // SHOULD NOT OCCUR
int id = d->efc_id[i];
@@ -1514,7 +1514,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
case mjEQ_WELD:
// cfrc = world-oriented torque:force vector
mju_copy3(cfrc + 3, d->efc_force + i);
if (m->eq_type[id]==mjEQ_WELD) {
if (m->eq_type[id] == mjEQ_WELD) {
mju_copy3(cfrc, d->efc_force + i + 3);
} else {
mju_zero3(cfrc); // no torque from connect
@@ -1523,7 +1523,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 1
if ((k = m->eq_obj1id[id])) {
// transform point on body1: local -> global
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_WELD), 0, k, 0);
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_WELD), 0, k, 0);
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
@@ -1535,7 +1535,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 2
if ((k = m->eq_obj2id[id])) {
// transform point on body2: local -> global
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_CONNECT), 0, k, 0);
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_CONNECT), 0, k, 0);
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
@@ -1545,7 +1545,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// increment rows
i += m->eq_type[id]==mjEQ_WELD ? 6 : 3;
i += m->eq_type[id] == mjEQ_WELD ? 6 : 3;
break;
case mjEQ_JOINT:
@@ -1562,7 +1562,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// forward pass over bodies: compute cacc, cfrc_int
mjtNum cacc[6], cfrc_body[6], cfrc_corr[6];
mju_zero(d->cfrc_int, 6);
for (int j=1; j<m->nbody; j++) {
for (int j=1; j < m->nbody; j++) {
// get body's first dof address
int bda = m->body_dofadr[j];
@@ -1583,7 +1583,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// backward pass over bodies: accumulate cfrc_int from children
for (int j=m->nbody-1; j>0; j--) {
for (int j=m->nbody-1; j > 0; j--) {
mju_addTo(d->cfrc_int+6*m->body_parentid[j], d->cfrc_int+6*j, 6);
}
}