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

Miscellaneous linting in src/engine.

Browse Source 
PiperOrigin-RevId: 462415459
Change-Id: I6c79c592613c6b0a665af06faf945586bb6ba32d
This commit is contained in:
Saran Tunyasuvunakool
2022-07-21 10:16:08 -07:00
committed by Copybara-Service
parent eef473b1e1
commit b849023069
19 changed files with 329 additions and 416 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_box.c
+165 -263
View File
@@ -24,15 +24,15 @@ static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n)
int i;
// loop over active limits
for (i = 0; i < n; i++)
if (limit[i] > 0)
{
for (i = 0; i < n; i++) {
if (limit[i] > 0) {
if (vec[i] < -limit[i])
vec[i] = -limit[i];
else if (vec[i] > limit[i])
vec[i] = limit[i];
}
}
}
static int _SphereBox(mjContact* con, mjtNum mindist,
@@ -59,8 +59,8 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
return 0;
if (dist <= mjMINVAL) // sphere center inside box
{
// sphere center inside box
if (dist <= mjMINVAL) {
closest = (size2[0] + size2[1] + size2[2]) * 2;
for (i = 0; i < 6; i++)
@@ -76,10 +76,7 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
mju_copy3(pos, center);
mju_addToScl3(pos, nearest, (size1[0] - closest) / 2);
mju_rotVecMat(con[0].frame, nearest, mat2);
}
else
{
} else {
mju_addToScl3(deepest, tmp, size1[0]);
mju_zero3(pos);
mju_addToScl3(pos, clamped, 0.5);
@@ -96,11 +93,11 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
}
int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum mindist)
int g1, int g2, mjtNum margin)
{
mjGETINFO;
return _SphereBox(con, mindist, pos1, mat1, size1, pos2, mat2, size2);
return _SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
@@ -117,7 +114,7 @@ int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con,
// a picture to see what is happening at each line of the code
int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum mindist)
int g1, int g2, mjtNum margin)
{
mjGETINFO
@@ -128,7 +125,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
mjtNum bestdist; // closest contact point distance
mjtNum bestdistmax; // init value for bestdist
mjtNum bestsegmentpos; // between -1 and 1 : which point on the segment is closest to the box
mjtNum secondpos; // distance of 2nd contact position on capsule segment from the first: same meaning
mjtNum secondpos; // distance of 2nd contact position on capsule segment from the first
mjtNum dist;
mjtNum bestboxpos; // closest contact point, position on the box's edge
mjtNum mul, e1, e2, dp, de;
@@ -150,8 +147,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
halflength = size1[1];
secondpos = -4; // initialize to no 2nd contact (valid values are between -1 and 1)
mju_sub3(tmp1, pos1,
pos2); // bring capsule to box-local frame (center's box is at (0,0,0))
mju_sub3(tmp1, pos1, pos2); // bring capsule to box-local frame (center's box is at (0,0,0))
mju_rotVecMatT(pos, tmp1, mat2); // and axis parralel to world
tmp1[0] = mat1[2]; // capsule's axis
@@ -169,9 +165,10 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
if (halfaxis[2] > 0)
axisdir += 4;
// under this notion "axisdir" and "7-axisdir" point in opposite directions, essentially the same for a capsule
// under this notion "axisdir" and "7-axisdir" point in opposite directions,
// essentially the same for a capsule
bestdistmax = mindist + 2 * (size1[0] + halflength + size2[0] + size2[1] +
bestdistmax = margin + 2 * (size1[0] + halflength + size2[0] + size2[1] +
size2[2]); // initialize bestdist
bestdist = bestdistmax;
bestsegmentpos = 0;
@@ -179,26 +176,22 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
mju_zero3(tmp2);
// test to see if maybe the a face of the box is closest to the capsule
for (i = -1; i <= 1; i += 2)
{
for (i = -1; i <= 1; i += 2) {
mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, i);
mju_copy3(tmp2, tmp1);
for (c1 = 0, j = 0, c2 = -1; j < 3; j++)
if (tmp1[j] < -size2[j])
{
for (c1 = 0, j = 0, c2 = -1; j < 3; j++) {
if (tmp1[j] < -size2[j]) {
c1++;
c2 = j;
tmp1[j] = -size2[j];
}
else if (tmp1[j] > size2[j])
{
} else if (tmp1[j] > size2[j]) {
c1++;
c2 = j;
tmp1[j] = size2[j];
}
}
if (c1 > 1)
continue;
@@ -206,8 +199,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
mju_subFrom3(tmp1, tmp2);
dist = mju_dot3(tmp1, tmp1);
if (dist < bestdist)
{
if (dist < bestdist) {
bestdist = dist;
bestsegmentpos = i;
cltype = -2 + i;
@@ -215,16 +207,11 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
}
}
mju_zero3(tmp2);
for (j = 0; j < 3; j++)
for (i = 0; i < 8; i++)
if ((i & (1 << j)) == 0)
{
for (j = 0; j < 3; j++) {
for (i = 0; i < 8; i++) {
if ((i & (1 << j)) == 0) {
// trick to get a corner
tmp3[0] = ((i & 1) ? 1 : -1) * size2[0];
tmp3[1] = ((i & 2) ? 1 : -1) * size2[1];
@@ -260,21 +247,17 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
s1 = s2 = 1;
if (x1 > 1)
{
if (x1 > 1) {
x1 = 1;
s1 = 2;
x2 = (v - mb) * (1 / mc);
}
else if (x1 < -1)
{
} else if (x1 < -1) {
x1 = -1;
s1 = 0;
x2 = (v + mb) * (1 / mc);
}
if (x2 > 1)
{
if (x2 > 1) {
x2 = 1;
s2 = 2;
x1 = (u - mb) * (1 / ma);
@@ -282,9 +265,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
x1 = 1, s1 = 2;
else if (x1 < -1)
x1 = -1, s1 = 0;
}
else if (x2 < -1)
{
} else if (x2 < -1) {
x2 = -1;
s2 = 0;
x1 = (u + mb) * (1 / ma);
@@ -294,8 +275,6 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
x1 = -1, s1 = 0;
}
mju_sub3(dif, tmp3, pos);
mju_addToScl3(dif, halfaxis, -x2);
@@ -306,34 +285,33 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
c1 = s1 * 3 + s2;
// the -MINVAL might not be necessary. Fixes numerical problem when axis is numerically parallel to the box
if (tmp1[2] < bestdist - mjMINVAL)
{
// the -MINVAL might not be necessary. Fixes numerical problem when axis is numerically
// parallel to the box
if (tmp1[2] < bestdist - mjMINVAL) {
bestdist = tmp1[2];
bestsegmentpos = x2;
bestboxpos = x1;
c2 = c1 / 6; // c1<6 means that closest point on the box is at the lower end or in the middle of the edge
// c1<6 means that closest point on the box is at the lower end
// or in the middle of the edge
c2 = c1 / 6;
clcorner = i + (1 << j) * c2; // which corner is the closest
cledge = j; // which axis
cltype = c1; // save clamped info
}
}
}
}
// penetration = -bestdist;
for (j = 0; j < 3; j++)
if (j == 2)
{
typedef union
{
struct
{
for (j = 0; j < 3; j++) {
if (j == 2) {
typedef union {
struct {
mjtNum x, y;
};
mjtNum c[2];
@@ -343,7 +321,6 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
bestdist = bestdistmax;
p.x = pos[0];
p.y = pos[1];
d.x = halfaxis[0];
@@ -361,33 +338,33 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
best = -1;
e1 = +u - v;
if ((e1 < 0) == (w < 0))
if (best < mju_abs(e1))
{
if ((e1 < 0) == (w < 0)) {
if (best < mju_abs(e1)) {
best = mju_abs(e1);
c1 = 0;
}
}
e1 = -u - v;
if ((e1 < 0) == (w < 0))
if (best < mju_abs(e1))
{
if ((e1 < 0) == (w < 0)) {
if (best < mju_abs(e1)) {
best = mju_abs(e1);
c1 = 1;
}
}
e1 = +u + v;
if ((e1 < 0) == (w < 0))
if (best < mju_abs(e1))
{
if ((e1 < 0) == (w < 0)) {
if (best < mju_abs(e1)) {
best = mju_abs(e1);
c1 = 2;
}
}
e1 = -u + v;
if ((e1 < 0) == (w < 0))
if (best < mju_abs(e1))
{
if ((e1 < 0) == (w < 0)) {
if (best < mju_abs(e1)) {
best = mju_abs(e1);
c1 = 3;
}
}
c.x = s.x * ((c1 / 2) ? -1 : 1);
c.y = s.y * ((c1 % 2) ? -1 : 1);
@@ -413,14 +390,8 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
// printf("%g %g %g %g\n",e1,e2,e3,e4);
}
}
// goto skip; // allow only the closest contact
@@ -438,8 +409,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
if (cltype == -4)
return 0;
if (cltype >= 0 && cltype / 3 != 1) // closest to a corner of the box
{
if (cltype >= 0 && cltype / 3 != 1) { // closest to a corner of the box
c1 = axisdir ^ clcorner;
// hack to find the relative orientation of capsule and corner
@@ -451,14 +421,13 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
if (c1 == 0 || c1 == 7)
goto skip; // case 1: no chance of additional contact
if (c1 == 1 || c1 == 2 || c1 == 4)
{
if (c1 == 1 || c1 == 2 || c1 == 4) {
mul = 1;
de = 1 - bestsegmentpos;
dp = 1 + bestsegmentpos;
}
if (c1 == 3 || c1 == 5 || c1 == 6)
{
if (c1 == 3 || c1 == 5 || c1 == 6) {
mul = -1;
c1 = 7 - c1;
dp = 1 - bestsegmentpos;
@@ -477,17 +446,15 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
if (axis[ax]*axis[ax] > 0.5) // second point along the edge of the box
{
if (axis[ax]*axis[ax] > 0.5) { // second point along the edge of the box
secondpos = de; // initial position from the
e1 = 2 * size2[ax] / fabs(halfaxis[ax]);
if (e1 < secondpos)
if (e1 < secondpos) {
secondpos = e1; // we overshoot, move back to the other corner of the edge
secondpos *= mul;
}
else // second point along a face of the box
{
secondpos *= mul;
} else { // second point along a face of the box
secondpos = dp;
// check for overshoot again
@@ -502,24 +469,18 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
secondpos *= -mul;
}
}
else if (cltype >= 0 && cltype / 3 == 1) // we are on box's edge
{
} else if (cltype >= 0 && cltype / 3 == 1) { // we are on box's edge
// hacks to find the relative orientation of capsule and edge
// there are 2 cases:
// c1= 2^n: edge and capsule are oriented in a T configuaration (no more contacts
// c1!=2^n: oriented in a cross X configuration
c1 = axisdir ^ clcorner; // same trick
c1 &= 7 - (1 << cledge); // even more hacks
//printf("%d %d %d %d %lf %lf %lf\n",axisdir,clcorner,c1,cledge,halfaxis[0],halfaxis[1],halfaxis[2]);
// printf("%d %d %d %d %lf %lf %lf\n",
// axisdir,clcorner,c1,cledge,halfaxis[0],halfaxis[1],halfaxis[2]);
if (c1 != 1 && c1 != 2 && c1 != 4)
goto skip;
@@ -540,23 +501,21 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
ax1 = ax2;
ax2 = 3 - ax - ax1;
// keep track of the axis orientation (mul will tell us which direction along the capsule to find the second point)
// you can notice all other references to the axis "halfaxis" are with absolute value
// keep track of the axis orientation (mul will tell us which direction along the capsule to
// find the second point) you can notice all other references to the axis "halfaxis" are with
// absolute value
if (c1 & (1 << ax2))
{
if (c1 & (1 << ax2)) {
mul = 1;
secondpos = 1 - bestsegmentpos;
}
else
{
} else {
mul = -1;
secondpos = 1 + bestsegmentpos;
}
// now we have to find out whether we point towards the opposite side or towards one of the sides
// and also find the farthest point along the capsule that is above the box
// now we have to find out whether we point towards the opposite side or towards one of the
// sides and also find the farthest point along the capsule that is above the box
e1 = 2 * size2[ax2] / fabs(halfaxis[ax2]);
if (e1 < secondpos)
@@ -573,10 +532,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
secondpos = e1;
secondpos *= mul;
}
else if (cltype < 0)
{
} else if (cltype < 0) {
// similarly we handle the case when one capsule's end is closest to a face of the box
// and find where is the other end pointing to and clamping to the farthest point
// of the capsule that's above the box
@@ -593,9 +549,8 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, -mul);
for (i = 0; i < 3; i++)
if (i != clface)
{
for (i = 0; i < 3; i++) {
if (i != clface) {
e1 = (size2[i] - tmp1[i]) / halfaxis[i] * mul;
if (e1 > 0)
if (e1 < secondpos)
@@ -605,7 +560,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
if (e1 > 0)
if (e1 < secondpos)
secondpos = e1;
}
}
secondpos *= mul;
}
@@ -620,16 +575,15 @@ skip:
mju_addTo3(tmp2, pos2);
// collide with
n = _SphereBox(con, mindist, tmp2, mat1, size1, pos2, mat2, size2);
n = _SphereBox(con, margin, tmp2, mat1, size1, pos2, mat2, size2);
if (secondpos > -3) // secondpos was modified
{
if (secondpos > -3) { // secondpos was modified
mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation
mju_rotVecMat(tmp2, tmp1, mat2);
mju_addTo3(tmp2, pos2);
n += _SphereBox(con + n, mindist, tmp2, mat1, size1, pos2, mat2, size2);
n += _SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2);
}
return n;
@@ -638,7 +592,7 @@ skip:
int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, mjtNum mindist)
int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, mjtNum margin)
{
const mjtNum* pos1 = D->geom_xpos + 3 * g1;
const mjtNum* mat1 = D->geom_xmat + 9 * g1;
@@ -652,16 +606,17 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
mjtNum rotmore[9], p[3], r[9], s[3], ss[3], lp[3], rt[9], points[mjMAXCONPAIR][3],
depth[mjMAXCONPAIR], pts[6][3], ppts2[4][2], pu[4][3], axi[3][3];
mjtNum linesu[4][6], lines[4][6], clnorm[3], rnorm[3];
mjtNum penetration, c1, c2, c3, a, b, c, d, lx, ly, hz, l, x, y, u, v, llx, lly, innorm, mindist2;
mjtNum penetration, c1, c2, c3, a, b, c, d, lx, ly, hz, l, x, y, u, v, llx, lly, innorm, margin2;
int i0, i1, i2;
mjtNum f0, f1, f2;
int i, j, q, code, q1, q2, clcorner, n, m, k, cle1, cle2, in, ax1, ax2, pax1, pax2, clface, nl, nf;
int i, j, q, code, q1, q2, clcorner, n, m, k;
int cle1, cle2, in, ax1, ax2, pax1, pax2, clface, nl, nf;
n = 0;
code = -1;
mindist2 = mindist * mindist;
margin2 = margin * margin;
mju_sub3(tmp1, pos2, pos1);
mju_rotVecMatT(pos21, tmp1, mat1);
@@ -680,24 +635,21 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
mju_rotVecMat(plen2, size2, rotabs);
mju_rotVecMatT(plen1, size1, rotabs);
for (i = 0, penetration = mindist; i < 3; i++)
for (i = 0, penetration = margin; i < 3; i++)
penetration += size1[i] * 3 + size2[i] * 3;
for (i = 0; i < 3; i++)
{
for (i = 0; i < 3; i++) {
c1 = -fabs(pos21[i]) + size1[i] + plen2[i];
c2 = -fabs(pos12[i]) + size2[i] + plen1[i];
if (c1 < -mindist || c2 < -mindist)
if (c1 < -margin || c2 < -margin)
return 0;
if (c1 < penetration)
{
if (c1 < penetration) {
penetration = c1;
code = i + 3 * (pos21[i] < 0) + 0;
}
if (c2 < penetration)
{
if (c2 < penetration) {
penetration = c2;
code = i + 3 * (pos12[i] < 0) + 6;
}
@@ -705,22 +657,16 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
// printf("%24.16e %24.16e %d %24.16e %d \n",c1,c2,i,penetration,code);
}
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
{
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
mju_zero3(tmp2);
if (i == 0)
{
if (i == 0) {
tmp2[1] = -rott[3 * j + 2];
tmp2[2] = +rott[3 * j + 1];
}
else if (i == 1)
{
} else if (i == 1) {
tmp2[0] = +rott[3 * j + 2];
tmp2[2] = -rott[3 * j + 0];
}
else if (i == 2)
{
} else if (i == 2) {
tmp2[0] = -rott[3 * j + 1];
tmp2[1] = +rott[3 * j + 0];
}
@@ -744,7 +690,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
c3 -= fabs(c2);
if (c3 < -mindist)
if (c3 < -margin)
return 0;
@@ -767,7 +713,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
}
// printf("%24.16e %d %24.16e %d\n",c3,12+i*3+j,penetration,code);
}
}
@@ -807,43 +753,43 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
i2 = 2;
f0 = f1 = f2 = 1;
if (q1 == 0)
{
if (q1 == 0) {
i0 = 2;
f0 = -1;
i2 = 0;
}
else if (q1 == 1)
{
} else if (q1 == 1) {
i1 = 2;
f1 = -1;
i2 = 1;
}
else if (q1 == 2) {}
else if (q1 == 3)
{
} else if (q1 == 2) {
} else if (q1 == 3) {
i0 = 2;
i2 = 0;
f2 = -1;
}
else if (q1 == 4)
{
} else if (q1 == 4) {
i1 = 2;
i2 = 1;
f2 = -1;
}
else if (q1 == 5)
{
} else if (q1 == 5) {
f0 = -1;
f2 = -1;
}
#define rotaxis(vecres,vecin) { vecres[0]=vecin[i0]*f0; vecres[1]=vecin[i1]*f1; vecres[2]=vecin[i2]*f2; }
#define rotmatx(matres,matin) { mju_scl3(matres+0,matin+i0*3,f0); mju_scl3(matres+3,matin+i1*3,f1); mju_scl3(matres+6,matin+i2*3,f2); }
#define rotaxis(vecres, vecin) \
{ \
vecres[0]=vecin[i0]*f0; \
vecres[1]=vecin[i1]*f1; \
vecres[2]=vecin[i2]*f2; \
}
#define rotmatx(matres, matin) \
{ \
mju_scl3(matres+0, matin+i0*3, f0); \
mju_scl3(matres+3, matin+i1*3, f1); \
mju_scl3(matres+6, matin+i2*3, f2); \
}
if (q2)
{
if (q2) {
mju_mulMatMatT(r, rotmore, rot, 3, 3, 3);
// mju_rotVecMat(p,pos12,rotmore);
@@ -853,9 +799,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
rotaxis(tmp1, size2);
mju_copy3(s, size1);
}
else
{
} else {
// mju_mulMatMat(r,rotmore,rot,3,3,3);
rotmatx(r, rot);
@@ -915,20 +859,15 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
mju_copy3(lines[k++] + 3, pts[1]);
}
for (i = 0; i < k; i++)
for (q = 0; q < 2; q++)
{
for (i = 0; i < k; i++) {
for (q = 0; q < 2; q++) {
a = lines[i][0 + q];
b = lines[i][3 + q];
c = lines[i][1 - q];
d = lines[i][4 - q];
if (fabs(b) > mjMINVAL)
for (j = -1; j <= 1; j += 2)
{
if (fabs(b) > mjMINVAL) {
for (j = -1; j <= 1; j += 2) {
l = ss[q] * j;
c1 = (l - a) * (1 / b);
if (c1 < 0 || c1 > 1)
@@ -939,7 +878,8 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
mju_copy3(points[n], lines[i]);
mju_addToScl3(points[n++], lines[i] + 3, c1);
}
}
}
}
@@ -951,9 +891,8 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
c1 = a * d - b * c;
if (m > 2)
for (i = 0; i < 4; i++)
{
if (m > 2) {
for (i = 0; i < 4; i++) {
llx = i / 2 ? lx : -lx;
lly = i % 2 ? ly : -ly;
@@ -969,11 +908,10 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
points[n][1] = lly;
points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]);
n++;
}
}
for (i = 0; i < (1 << (m - 1)); i++)
{
for (i = 0; i < (1 << (m - 1)); i++) {
mju_copy3(tmp1, pts[i == 0 ? 0 : i + 2]);
@@ -985,7 +923,6 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
continue;
mju_copy3(points[n++], tmp1);
}
@@ -994,7 +931,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
for (i = 0; i < m; i++)
{
if (points[i][2] > mindist)
if (points[i][2] > margin)
continue;
mju_copy3(points[n], points[i]);
@@ -1062,13 +999,11 @@ edgeedge:
// printf("%lf %lf %lf %lf\n",rot[ 3*q1+ ax1],rot [3*q1+ ax2],rott[3*q2+pax1],rott[3*q2+pax2]);
// printf("%lf %lf\n",mju_dot3(clnorm,rott+3*ax1),mju_dot3(clnorm,rott+3*ax2));
if (rotabs [3 * q1 + ax1] < rotabs [3 * q1 + ax2])
{
if (rotabs [3 * q1 + ax1] < rotabs [3 * q1 + ax2]) {
ax1 = ax2;
ax2 = 3 - q2 - ax1;
}
if (rottabs[3 * q2 + pax1] < rottabs[3 * q2 + pax2])
{
if (rottabs[3 * q2 + pax1] < rottabs[3 * q2 + pax2]) {
pax1 = pax2;
pax2 = 3 - q1 - pax1;
}
@@ -1079,7 +1014,8 @@ edgeedge:
clface = pax2 + 3;
//printf("%lf - %d %d %d %d %d %d %d %d %d %d %d\n",penetration,cle1,cle2,code,in,q1,q2,clface,ax1,ax2,pax1,pax2);
// printf("%lf - %d %d %d %d %d %d %d %d %d %d %d\n",
// penetration,cle1,cle2,code,in,q1,q2,clface,ax1,ax2,pax1,pax2);
mju_zero(rotmore, 9);
@@ -1102,41 +1038,28 @@ edgeedge:
i2 = 2;
f0 = f1 = f2 = 1;
if (clface == 0)
{
if (clface == 0) {
i0 = 2;
f0 = -1;
i2 = 0;
}
else if (clface == 1)
{
} else if (clface == 1) {
i1 = 2;
f1 = -1;
i2 = 1;
}
else if (clface == 2) {}
else if (clface == 3)
{
} else if (clface == 2) {
} else if (clface == 3) {
i0 = 2;
i2 = 0;
f2 = -1;
}
else if (clface == 4)
{
} else if (clface == 4) {
i1 = 2;
i2 = 1;
f2 = -1;
}
else if (clface == 5)
{
} else if (clface == 5) {
f0 = -1;
f2 = -1;
}
// mju_rotVecMat(p,pos21,rotmore);
// mju_rotVecMat(rnorm,clnorm,rotmore);
rotaxis(p, pos21);
@@ -1204,7 +1127,6 @@ edgeedge:
// ppts[i][1]=points[i][1];
ppts2[i][0] = points[i][0];
ppts2[i][1] = points[i][1];
}
@@ -1217,15 +1139,13 @@ edgeedge:
n = 0;
if (m > 1)
{
if (m > 1) {
mju_copy3(lines[k] + 0, pts[0]);
mju_copy3(lines[k] + 3, pts[1]);
mju_copy3(linesu[k] + 0, axi[0]);
mju_copy3(linesu[k++] + 3, axi[1]);
}
if (m > 2)
{
if (m > 2) {
mju_copy3(lines[k] + 0, pts[0]);
mju_copy3(lines[k] + 3, pts[2]);
mju_copy3(linesu[k] + 0, axi[0]);
@@ -1242,19 +1162,16 @@ edgeedge:
mju_copy3(linesu[k++] + 3, axi[1]);
}
for (i = 0; i < k; i++)
for (q = 0; q < 2; q++)
{
for (i = 0; i < k; i++) {
for (q = 0; q < 2; q++) {
a = lines[i][0 + q];
b = lines[i][3 + q];
c = lines[i][1 - q];
d = lines[i][4 - q];
if (fabs(b) > mjMINVAL)
for (j = -1; j <= 1; j += 2)
if (n < mjMAXCONPAIR)
{
if (fabs(b) > mjMINVAL) {
for (j = -1; j <= 1; j += 2) {
if (n < mjMAXCONPAIR) {
l = s[q] * j;
c1 = (l - a) * (1 / b);
if (c1 < 0 || c1 > 1)
@@ -1263,7 +1180,7 @@ edgeedge:
if (fabs(c2) > s[1 - q])
continue;
if ((linesu[i][2] + linesu[i][5]*c1)*innorm > mindist)
if ((linesu[i][2] + linesu[i][5]*c1)*innorm > margin)
continue;
mju_scl3(points[n], linesu[i], 0.5);
@@ -1272,9 +1189,10 @@ edgeedge:
points[n][1 - q] += 0.5 * c2;
depth[n] = points[n][2] * innorm * 2;
n++;
}
}
}
}
}
nl = n;
@@ -1285,11 +1203,8 @@ edgeedge:
d = pts[2][1];
c1 = a * d - b * c;
for (i = 0; i < 4; i++)
if (n < mjMAXCONPAIR)
{
for (i = 0; i < 4; i++) {
if (n < mjMAXCONPAIR) {
llx = i / 2 ? lx : -lx;
lly = i % 2 ? ly : -ly;
@@ -1299,15 +1214,10 @@ edgeedge:
u = (x * d - y * b) * (1 / c1);
v = (y * a - x * c) * (1 / c1);
if (nl == 0)
{
if (nl == 0) {
if ((u < 0 || u > 1) && (v < 0 || v > 1))
continue;
}
else
{
} else {
if ((u < 0 || u > 1 || v < 0 || v > 1))
continue;
}
@@ -1334,37 +1244,32 @@ edgeedge:
c1 = mju_dot3(tmp2, tmp2);
if (tmp1[2] > 0)
if (c1 > mindist2)
if (c1 > margin2)
continue;
mju_add3(points[n], points[n], tmp1);
mju_scl3(points[n], points[n], 0.5);
depth[n] = sqrt(c1) * (tmp1[2] < 0 ? -1 : 1);;
depth[n] = sqrt(c1) * (tmp1[2] < 0 ? -1 : 1);
n++;
}
}
nf = n;
for (i = 0; i < 4; i++)
if (n < mjMAXCONPAIR)
{
for (i = 0; i < 4; i++) {
if (n < mjMAXCONPAIR) {
x = ppts2[i][0];
y = ppts2[i][1];
if (nl == 0)
if (nf == 0) {}
else
{
if (nl == 0) {
if (nf == 0) {
} else {
if (x < -lx || x > lx)
if (y < -ly || y > ly)
continue;
}
else
{
} else {
if (x < -lx || x > lx || y < -ly || y > ly)
continue;
}
@@ -1378,7 +1283,7 @@ edgeedge:
c1 += pu[i][2] * innorm * pu[i][2] * innorm;
if (pu[i][2] > 0)
if (c1 > mindist2)
if (c1 > margin2)
continue;
@@ -1386,17 +1291,18 @@ edgeedge:
tmp1[1] = ppts2[i][1] * 0.5;
tmp1[2] = 0;
for (j = 0; j < 2; j++)
for (j = 0; j < 2; j++) {
if (ppts2[i][j] < -s[j])
tmp1[j] = -s[j] * 0.5;
else if (ppts2[i][j] > s[j])
tmp1[j] = +s[j] * 0.5;
}
mju_addToScl3(tmp1, pu[i], 0.5);
mju_copy3(points[n], tmp1);
depth[n] = sqrt(c1) * (pu[i][2] < 0 ? -1 : 1);
n++;
}
}
mju_mulMatMatT(r, mat1, rotmore, 3, 3, 3);
@@ -1407,8 +1313,7 @@ edgeedge:
mju_zero3(con[0].frame + 3);
for (i = 0; i < n; i++)
{
for (i = 0; i < n; i++) {
con[i].dist = depth[i];
points[i][2] += hz;
@@ -1417,11 +1322,8 @@ edgeedge:
mju_add3(con[i].pos, tmp2, pos1);
mju_copy(con[i].frame, con[0].frame, 6);
}
return n;
#undef rotaxis
src/engine/engine_collision_convex.c
+2 -2
View File
@@ -183,10 +183,11 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
}
// copy best vertex
else
else {
for (int i=0; i<3; i++) {
res[i] = (mjtNum)vertdata[3*ibest + i];
}
}
break;
default:
@@ -298,7 +299,6 @@ int mjc_Convex(const mjModel* m, const mjData* d,
if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?)
&& m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE
&& m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) {
// multiCCD parameters
const mjtNum relative_tolerance = 1e-3;
const mjtNum perturbation_angle = 1e-3;
src/engine/engine_collision_driver.c
+2 -3
View File
@@ -34,7 +34,6 @@
// table of pair-wise collision functions
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH */
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex},
/*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField},
@@ -44,7 +43,6 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
/*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex},
/*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex},
/*MESH */ {0, 0, 0, 0, 0, 0, 0, mjc_Convex}
};
@@ -146,11 +144,12 @@ void mj_collision(const mjModel* m, mjData* d) {
}
// finish merging predefined pairs
if (npair && m->opt.collision==mjCOL_ALL)
if (npair && m->opt.collision==mjCOL_ALL) {
while (pairadr<npair) {
mj_collideGeoms(m, d, pairadr++, -1, 0, 0);
}
}
}
mjFREESTACK;
}
src/engine/engine_core_constraint.c
+6 -4
View File
@@ -145,12 +145,14 @@ int mj_addConstraint(const mjModel* m, mjData* d,
// dense: copy entire Jacobian
if (!mj_isSparse(m)) {
// make sure jac is not empty
if (empty)
for (int i=0; i<size*nv; i++)
if (empty) {
for (int i=0; i<size*nv; i++) {
if (jac[i]) {
empty = 0;
break;
}
}
}
// copy if not empty
if (!empty) {
@@ -506,11 +508,11 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
deriv = data[1] + 2*data[2]*dif + 3*data[3]*dif*dif + 4*data[4]*dif*dif*dif;
// compute Jacobian: sparse or dense
if (issparse)
if (issparse) {
NV = mju_combineSparse(jac[0], jac[1], nv, 1, -deriv,
NV, NV2, chain, chain2,
sparse_buf, buf_ind);
else {
} else {
mju_addToScl(jac[0], jac[1], -deriv, nv);
}
}
src/engine/engine_core_constraint.h
+2 -2
View File
@@ -63,10 +63,10 @@ int mj_addConstraint(const mjModel* m, mjData* d,
int size, int type, int id, int NV, const int* chain);
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* dofid, int b1, int b2);
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2);
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* dofid, int b1, int b2);
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2);
//-------------------------- constraint instantiation ----------------------------------------------
src/engine/engine_core_smooth.c
-5
View File
@@ -178,7 +178,6 @@ void mj_kinematics(const mjModel* m, mjData* d) {
// map inertias and motion dofs to global frame centered at subtree-CoM
void mj_comPos(const mjModel* m, mjData* d) {
mjtNum offset[3], axis[3];
mjMARKSTACK;
mjtNum* mass_subtree = mj_stackAlloc(d, m->nbody);
@@ -1798,7 +1797,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 1
if ((k = m->eq_obj1id[id])) {
// transform connect point on body1: local -> global
mju_rotVecMat(pos, eq_data, d->xmat+9*k);
mju_addTo3(pos, d->xpos+3*k);
@@ -1812,7 +1810,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 2
if ((k = m->eq_obj2id[id])) {
// transform connect point on body2: local -> global
mju_rotVecMat(pos, eq_data + 3, d->xmat+9*k);
mju_addTo3(pos, d->xpos+3*k);
@@ -1836,7 +1833,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 1
if ((k = m->eq_obj1id[id])) {
// transform weld point on body1: local -> global
mju_rotVecMat(pos, eq_data, d->xmat+9*k);
mju_addTo3(pos, d->xpos+3*k);
@@ -1850,7 +1846,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 2
if ((k = m->eq_obj2id[id])) {
// weld force on body2 is always applied at body root
mju_copy3(pos, d->xpos+3*k);
src/engine/engine_derivative.c
+1 -3
View File
@@ -573,7 +573,6 @@ static void addJTBJSparse(mjtNum* DfDv, const mjtNum* J, const mjtNum* B,
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
if (B[i*n+j]) {
// process non-zero elements of J(i,k)
for (int k=0; k<rownnz[offset+i]; k++) {
int ik = rowadr[offset+i] + k;
@@ -758,7 +757,7 @@ static void mjd_addedMassForces(
mjtNum* restrict B, const mjtNum local_vels[6], const mjtNum fluid_density,
const mjtNum virtual_mass[3], const mjtNum virtual_inertia[3]) {
const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]};
const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]};;
const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]};
const mjtNum virtual_lin_mom[3] = {
fluid_density * virtual_mass[0] * lin_vel[0],
fluid_density * virtual_mass[1] * lin_vel[1],
@@ -1567,7 +1566,6 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte centered,
// finite-difference activations: skip=mjSTAGE_VEL
if (DyDa || DsDa) {
for (int i=0; i<na; i++) {
// nudge forward
d->act[i] += eps;
src/engine/engine_file.c
+1 -1
View File
@@ -35,7 +35,7 @@ void* mju_fileToMemory(const char* filename, int* filesize) {
}
// ensure file size fits in int
long long_filesize = ftell(fp);
long long_filesize = ftell(fp); // NOLINT(runtime/int)
if (long_filesize > INT_MAX) {
fclose(fp);
mju_warning_s("File size over 2GB is not supported. File: '%s'", filename);
src/engine/engine_forward.c
-1
View File
@@ -485,7 +485,6 @@ static void mj_advance(const mjModel* m, mjData* d,
// Euler integrator, semi-implicit in velocity, possibly skipping factorisation
void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
int i, nv = m->nv, nM = m->nM;
mjMARKSTACK;
mjtNum* qfrc = mj_stackAlloc(d, nv);
src/engine/engine_io.c
-1
View File
@@ -1181,7 +1181,6 @@ static int numObjects(const mjModel* m, mjtObj objtype) {
// validate reference fields in a model; return null if valid, error message otherwise
const char* mj_validateReferences(const mjModel* m) {
// for each field in mjModel that refers to another field, call X with:
// adrarray: array containing the references
// nadrs: number of elements in refarray
src/engine/engine_macro.h
+1 -1
View File
@@ -15,7 +15,7 @@
#ifndef MUJOCO_SRC_ENGINE_ENGINE_MACRO_H_
#define MUJOCO_SRC_ENGINE_ENGINE_MACRO_H_
#include "engine/engine_callback.h"
#include "engine/engine_callback.h" // IWYU pragma: export
//-------------------------------- utility macros --------------------------------------------------
src/engine/engine_sensor.c
+2 -2
View File
@@ -47,7 +47,7 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
// real or positive: add noise directly, with clamp for positive
if (m->sensor_datatype[i]==mjDATATYPE_REAL ||
m->sensor_datatype[i]==mjDATATYPE_POSITIVE)
m->sensor_datatype[i]==mjDATATYPE_POSITIVE) {
for (int j=0; j<dim; j++) {
// get random numbers; use only the first one
rnd[0] = mju_standardNormal(rnd+1);
@@ -65,6 +65,7 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
d->sensordata[adr+j] += rnd[0]*noise;
}
}
}
// axis or quat: rotate around random axis by random angle
else {
@@ -114,7 +115,6 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
// process all dimensions
for (int j=0; j<dim; j++) {
// real: apply on both sides
if (m->sensor_datatype[i]==mjDATATYPE_REAL) {
d->sensordata[adr+j] = mju_clip(d->sensordata[adr+j], -cutoff, cutoff);
src/engine/engine_util_errmem.c
+16 -7
View File
@@ -102,15 +102,24 @@ void _mjPRIVATE__set_tls_warning_fn(callback_fn h) {
// write datetime, type: message to MUJOCO_LOG.TXT
void mju_writeLog(const char* type, const char* msg) {
time_t rawtime;
struct tm *timeinfo;
struct tm timeinfo;
FILE* fp = fopen("MUJOCO_LOG.TXT", "a+t");
if (fp) {
// get time
time(&rawtime);
timeinfo = localtime(&rawtime);
#if defined(_POSIX_C_SOURCE) || defined(__APPLE__) || defined(__STDC_VERSION_TIME_H__)
localtime_r(&rawtime, &timeinfo);
#elif _MSC_VER
localtime_s(&timeinfo, &rawtime);
#elif __STDC_LIB_EXT1__
localtime_s(&rawtime, &timeinfo);
#else
#error "Thread-safe version of `localtime` is not present in the standard C library"
#endif
// write to log file
fprintf(fp, "%s%s: %s\n\n", asctime(timeinfo), type, msg);
fprintf(fp, "%s%s: %s\n\n", asctime(&timeinfo), type, msg);
fclose(fp);
}
}
@@ -151,7 +160,7 @@ void mju_warning(const char* msg) {
// error with int argument
void mju_error_i(const char* msg, int i) {
char errmsg[1000];
snprintf(errmsg, 1000, msg, i);
snprintf(errmsg, sizeof(errmsg), msg, i);
errmsg[999] = '\0';
mju_error(errmsg);
}
@@ -160,7 +169,7 @@ void mju_error_i(const char* msg, int i) {
// warning with int argument
void mju_warning_i(const char* msg, int i) {
char wrnmsg[1000];
snprintf(wrnmsg, 1000, msg, i);
snprintf(wrnmsg, sizeof(wrnmsg), msg, i);
wrnmsg[999] = '\0';
mju_warning(wrnmsg);
}
@@ -169,7 +178,7 @@ void mju_warning_i(const char* msg, int i) {
// error string argument
void mju_error_s(const char* msg, const char* text) {
char errmsg[1000];
snprintf(errmsg, 1000, msg, text);
snprintf(errmsg, sizeof(errmsg), msg, text);
errmsg[999] = '\0';
mju_error(errmsg);
}
@@ -178,7 +187,7 @@ void mju_error_s(const char* msg, const char* text) {
// warning string argument
void mju_warning_s(const char* msg, const char* text) {
char wrnmsg[1000];
snprintf(wrnmsg, 1000, msg, text);
snprintf(wrnmsg, sizeof(wrnmsg), msg, text);
wrnmsg[999] = '\0';
mju_warning(wrnmsg);
}
src/engine/engine_util_solve.c
+4 -3
View File
@@ -18,7 +18,6 @@
#include <string.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_util_blas.h"
@@ -116,13 +115,15 @@ int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus) {
mat[k*(n+1)] = r;
// update mat
if (flg_plus)
if (flg_plus) {
for (int i=k+1; i<n; i++) {
mat[i*n+k] = (mat[i*n+k] + s*x[i])*cinv;
} else
}
} else {
for (int i=k+1; i<n; i++) {
mat[i*n+k] = (mat[i*n+k] - s*x[i])*cinv;
}
}
// update x
for (int i=k+1; i<n; i++) {
src/engine/engine_util_sparse.c
+5 -2
View File
@@ -262,16 +262,19 @@ void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
const int* colind, const int* rowsuper) {
// no supernodes, or no AVX
#ifdef mjUSEAVX
if (!rowsuper)
if (!rowsuper) {
#endif
{
// regular sparse dot-product
for (int r=0; r<nr; r++) {
res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
}
return;
#ifdef mjUSEAVX
}
#endif
// regular or supernode
for (int r=0; r<nr; r++) {
src/engine/engine_vis_visualize.c
+16 -11
View File
@@ -32,7 +32,9 @@
//----------------------------- utility functions and macros ---------------------------------------
static const mjtNum IDENTITY[9] = {1,0,0, 0,1,0, 0,0,1};
static const mjtNum IDENTITY[9] = {1, 0, 0,
0, 1, 0,
0, 0, 1};
// copy float array
@@ -65,15 +67,17 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) {
// return if there is no space in buffer
#define START \
if( scn->ngeom>=scn->maxgeom ) \
{ mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); \
return; } \
else { thisgeom = scn->geoms + scn->ngeom; \
if ( scn->ngeom>=scn->maxgeom ) { \
mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); \
return; \
} else { \
thisgeom = scn->geoms + scn->ngeom; \
mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); \
thisgeom->objtype = objtype; \
thisgeom->objid = i; \
thisgeom->category = category; \
thisgeom->segid = scn->ngeom; }
thisgeom->segid = scn->ngeom; \
}
// advance counter
@@ -327,7 +331,7 @@ void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size,
geom->type = type;
// set size (for XYZ scaling)
if (size)
if (size) {
switch (type) {
case mjGEOM_SPHERE:
geom->size[0] = (float)size[0];
@@ -349,6 +353,7 @@ void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size,
default:
mju_n2f(geom->size, size, 3);
}
} else {
geom->size[0] = 0.1f;
geom->size[1] = 0.1f;
@@ -838,7 +843,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (m->actuator_trntype[i]==mjTRN_JOINT ||
m->actuator_trntype[i]==mjTRN_JOINTINPARENT ||
m->actuator_trntype[i]==mjTRN_SITE) {
START
// site actuators
@@ -1355,8 +1359,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// slider-crank
objtype = mjOBJ_ACTUATOR;
category = mjCAT_DYNAMIC;
if ((category & catmask))
for (int i=0; i<m->nu; i++)
if ((category & catmask)) {
for (int i=0; i<m->nu; i++) {
if (m->actuator_trntype[i]==mjTRN_SLIDERCRANK) {
// get data
int j = m->actuator_trnid[2*i]; // crank
@@ -1404,6 +1408,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
FINISH
}
}
}
// center of mass for root bodies
objtype = mjOBJ_UNKNOWN;
@@ -1778,7 +1784,6 @@ void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
// update visible skins only
void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
// process skins
for (int i=0; i<m->nskin; i++) {
// get info
src/user/user_util.cc
+1
View File
@@ -20,6 +20,7 @@
#include <limits>
#include <string>
#include <mujoco/mjtnum.h>
#include "engine/engine_macro.h"
using std::isnan;