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; int i;
// loop over active limits // loop over active limits
for (i = 0; i < n; i++) for (i = 0; i < n; i++) {
if (limit[i] > 0) if (limit[i] > 0) {
{
if (vec[i] < -limit[i]) if (vec[i] < -limit[i])
vec[i] = -limit[i]; vec[i] = -limit[i];
else if (vec[i] > limit[i]) else if (vec[i] > limit[i])
vec[i] = limit[i]; vec[i] = limit[i];
} }
} }
}
static int _SphereBox(mjContact* con, mjtNum mindist, static int _SphereBox(mjContact* con, mjtNum mindist,
@@ -59,8 +59,8 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
return 0; return 0;
if (dist <= mjMINVAL) // sphere center inside box // sphere center inside box
{ if (dist <= mjMINVAL) {
closest = (size2[0] + size2[1] + size2[2]) * 2; closest = (size2[0] + size2[1] + size2[2]) * 2;
for (i = 0; i < 6; i++) for (i = 0; i < 6; i++)
@@ -76,10 +76,7 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
mju_copy3(pos, center); mju_copy3(pos, center);
mju_addToScl3(pos, nearest, (size1[0] - closest) / 2); mju_addToScl3(pos, nearest, (size1[0] - closest) / 2);
mju_rotVecMat(con[0].frame, nearest, mat2); mju_rotVecMat(con[0].frame, nearest, mat2);
} else {
}
else
{
mju_addToScl3(deepest, tmp, size1[0]); mju_addToScl3(deepest, tmp, size1[0]);
mju_zero3(pos); mju_zero3(pos);
mju_addToScl3(pos, clamped, 0.5); 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 mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum mindist) int g1, int g2, mjtNum margin)
{ {
mjGETINFO; 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 // 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 mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum mindist) int g1, int g2, mjtNum margin)
{ {
mjGETINFO mjGETINFO
@@ -128,7 +125,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
mjtNum bestdist; // closest contact point distance mjtNum bestdist; // closest contact point distance
mjtNum bestdistmax; // init value for bestdist mjtNum bestdistmax; // init value for bestdist
mjtNum bestsegmentpos; // between -1 and 1 : which point on the segment is closest to the box 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 dist;
mjtNum bestboxpos; // closest contact point, position on the box's edge mjtNum bestboxpos; // closest contact point, position on the box's edge
mjtNum mul, e1, e2, dp, de; mjtNum mul, e1, e2, dp, de;
@@ -150,8 +147,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
halflength = size1[1]; halflength = size1[1];
secondpos = -4; // initialize to no 2nd contact (valid values are between -1 and 1) secondpos = -4; // initialize to no 2nd contact (valid values are between -1 and 1)
mju_sub3(tmp1, pos1, mju_sub3(tmp1, pos1, pos2); // bring capsule to box-local frame (center's box is at (0,0,0))
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 mju_rotVecMatT(pos, tmp1, mat2); // and axis parralel to world
tmp1[0] = mat1[2]; // capsule's axis 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) if (halfaxis[2] > 0)
axisdir += 4; 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 size2[2]); // initialize bestdist
bestdist = bestdistmax; bestdist = bestdistmax;
bestsegmentpos = 0; bestsegmentpos = 0;
@@ -179,26 +176,22 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
mju_zero3(tmp2); mju_zero3(tmp2);
// test to see if maybe the a face of the box is closest to the capsule // 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_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, i); mju_addToScl3(tmp1, halfaxis, i);
mju_copy3(tmp2, tmp1); mju_copy3(tmp2, tmp1);
for (c1 = 0, j = 0, c2 = -1; j < 3; j++) for (c1 = 0, j = 0, c2 = -1; j < 3; j++) {
if (tmp1[j] < -size2[j]) if (tmp1[j] < -size2[j]) {
{
c1++; c1++;
c2 = j; c2 = j;
tmp1[j] = -size2[j]; tmp1[j] = -size2[j];
} } else if (tmp1[j] > size2[j]) {
else if (tmp1[j] > size2[j])
{
c1++; c1++;
c2 = j; c2 = j;
tmp1[j] = size2[j]; tmp1[j] = size2[j];
} }
}
if (c1 > 1) if (c1 > 1)
continue; continue;
@@ -206,8 +199,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
mju_subFrom3(tmp1, tmp2); mju_subFrom3(tmp1, tmp2);
dist = mju_dot3(tmp1, tmp1); dist = mju_dot3(tmp1, tmp1);
if (dist < bestdist) if (dist < bestdist) {
{
bestdist = dist; bestdist = dist;
bestsegmentpos = i; bestsegmentpos = i;
cltype = -2 + i; cltype = -2 + i;
@@ -215,16 +207,11 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
} }
} }
mju_zero3(tmp2); mju_zero3(tmp2);
for (j = 0; j < 3; j++) {
for (j = 0; j < 3; j++) for (i = 0; i < 8; i++) {
for (i = 0; i < 8; i++) if ((i & (1 << j)) == 0) {
if ((i & (1 << j)) == 0)
{
// trick to get a corner // trick to get a corner
tmp3[0] = ((i & 1) ? 1 : -1) * size2[0]; tmp3[0] = ((i & 1) ? 1 : -1) * size2[0];
tmp3[1] = ((i & 2) ? 1 : -1) * size2[1]; 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; s1 = s2 = 1;
if (x1 > 1) if (x1 > 1) {
{
x1 = 1; x1 = 1;
s1 = 2; s1 = 2;
x2 = (v - mb) * (1 / mc); x2 = (v - mb) * (1 / mc);
} } else if (x1 < -1) {
else if (x1 < -1)
{
x1 = -1; x1 = -1;
s1 = 0; s1 = 0;
x2 = (v + mb) * (1 / mc); x2 = (v + mb) * (1 / mc);
} }
if (x2 > 1) if (x2 > 1) {
{
x2 = 1; x2 = 1;
s2 = 2; s2 = 2;
x1 = (u - mb) * (1 / ma); x1 = (u - mb) * (1 / ma);
@@ -282,9 +265,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
x1 = 1, s1 = 2; x1 = 1, s1 = 2;
else if (x1 < -1) else if (x1 < -1)
x1 = -1, s1 = 0; x1 = -1, s1 = 0;
} } else if (x2 < -1) {
else if (x2 < -1)
{
x2 = -1; x2 = -1;
s2 = 0; s2 = 0;
x1 = (u + mb) * (1 / ma); x1 = (u + mb) * (1 / ma);
@@ -294,8 +275,6 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
x1 = -1, s1 = 0; x1 = -1, s1 = 0;
} }
mju_sub3(dif, tmp3, pos); mju_sub3(dif, tmp3, pos);
mju_addToScl3(dif, halfaxis, -x2); mju_addToScl3(dif, halfaxis, -x2);
@@ -306,34 +285,33 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
c1 = s1 * 3 + s2; c1 = s1 * 3 + s2;
// the -MINVAL might not be necessary. Fixes numerical problem when axis is numerically parallel to the box // the -MINVAL might not be necessary. Fixes numerical problem when axis is numerically
if (tmp1[2] < bestdist - mjMINVAL) // parallel to the box
{ if (tmp1[2] < bestdist - mjMINVAL) {
bestdist = tmp1[2]; bestdist = tmp1[2];
bestsegmentpos = x2; bestsegmentpos = x2;
bestboxpos = x1; 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 clcorner = i + (1 << j) * c2; // which corner is the closest
cledge = j; // which axis cledge = j; // which axis
cltype = c1; // save clamped info cltype = c1; // save clamped info
} }
}
}
} }
// penetration = -bestdist; // penetration = -bestdist;
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++) {
if (j == 2) if (j == 2) {
{ typedef union {
struct {
typedef union
{
struct
{
mjtNum x, y; mjtNum x, y;
}; };
mjtNum c[2]; mjtNum c[2];
@@ -343,7 +321,6 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
bestdist = bestdistmax; bestdist = bestdistmax;
p.x = pos[0]; p.x = pos[0];
p.y = pos[1]; p.y = pos[1];
d.x = halfaxis[0]; d.x = halfaxis[0];
@@ -361,33 +338,33 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
best = -1; best = -1;
e1 = +u - v; e1 = +u - v;
if ((e1 < 0) == (w < 0)) if ((e1 < 0) == (w < 0)) {
if (best < mju_abs(e1)) if (best < mju_abs(e1)) {
{
best = mju_abs(e1); best = mju_abs(e1);
c1 = 0; c1 = 0;
} }
}
e1 = -u - v; e1 = -u - v;
if ((e1 < 0) == (w < 0)) if ((e1 < 0) == (w < 0)) {
if (best < mju_abs(e1)) if (best < mju_abs(e1)) {
{
best = mju_abs(e1); best = mju_abs(e1);
c1 = 1; c1 = 1;
} }
}
e1 = +u + v; e1 = +u + v;
if ((e1 < 0) == (w < 0)) if ((e1 < 0) == (w < 0)) {
if (best < mju_abs(e1)) if (best < mju_abs(e1)) {
{
best = mju_abs(e1); best = mju_abs(e1);
c1 = 2; c1 = 2;
} }
}
e1 = -u + v; e1 = -u + v;
if ((e1 < 0) == (w < 0)) if ((e1 < 0) == (w < 0)) {
if (best < mju_abs(e1)) if (best < mju_abs(e1)) {
{
best = mju_abs(e1); best = mju_abs(e1);
c1 = 3; c1 = 3;
} }
}
c.x = s.x * ((c1 / 2) ? -1 : 1); c.x = s.x * ((c1 / 2) ? -1 : 1);
c.y = s.y * ((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); // printf("%g %g %g %g\n",e1,e2,e3,e4);
} }
}
// goto skip; // allow only the closest contact // 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) if (cltype == -4)
return 0; 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; c1 = axisdir ^ clcorner;
// hack to find the relative orientation of capsule and corner // 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) if (c1 == 0 || c1 == 7)
goto skip; // case 1: no chance of additional contact 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; mul = 1;
de = 1 - bestsegmentpos; de = 1 - bestsegmentpos;
dp = 1 + bestsegmentpos; dp = 1 + bestsegmentpos;
} }
if (c1 == 3 || c1 == 5 || c1 == 6)
{ if (c1 == 3 || c1 == 5 || c1 == 6) {
mul = -1; mul = -1;
c1 = 7 - c1; c1 = 7 - c1;
dp = 1 - bestsegmentpos; 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 secondpos = de; // initial position from the
e1 = 2 * size2[ax] / fabs(halfaxis[ax]); 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 = 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; secondpos = dp;
// check for overshoot again // check for overshoot again
@@ -502,24 +469,18 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
secondpos *= -mul; 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 // hacks to find the relative orientation of capsule and edge
// there are 2 cases: // there are 2 cases:
// c1= 2^n: edge and capsule are oriented in a T configuaration (no more contacts // 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!=2^n: oriented in a cross X configuration
c1 = axisdir ^ clcorner; // same trick c1 = axisdir ^ clcorner; // same trick
c1 &= 7 - (1 << cledge); // even more hacks 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) if (c1 != 1 && c1 != 2 && c1 != 4)
goto skip; goto skip;
@@ -540,23 +501,21 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
ax1 = ax2; ax1 = ax2;
ax2 = 3 - ax - ax1; ax2 = 3 - ax - ax1;
// keep track of the axis orientation (mul will tell us which direction along the capsule to find the second point) // keep track of the axis orientation (mul will tell us which direction along the capsule to
// you can notice all other references to the axis "halfaxis" are with absolute value // 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; mul = 1;
secondpos = 1 - bestsegmentpos; secondpos = 1 - bestsegmentpos;
} } else {
else
{
mul = -1; mul = -1;
secondpos = 1 + bestsegmentpos; secondpos = 1 + bestsegmentpos;
} }
// now we have to find out whether we point towards the opposite side or towards one of the sides // now we have to find out whether we point towards the opposite side or towards one of the
// and also find the farthest point along the capsule that is above the box // sides and also find the farthest point along the capsule that is above the box
e1 = 2 * size2[ax2] / fabs(halfaxis[ax2]); e1 = 2 * size2[ax2] / fabs(halfaxis[ax2]);
if (e1 < secondpos) if (e1 < secondpos)
@@ -573,10 +532,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
secondpos = e1; secondpos = e1;
secondpos *= mul; 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 // 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 // and find where is the other end pointing to and clamping to the farthest point
// of the capsule that's above the box // 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_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, -mul); mju_addToScl3(tmp1, halfaxis, -mul);
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++) {
if (i != clface) if (i != clface) {
{
e1 = (size2[i] - tmp1[i]) / halfaxis[i] * mul; e1 = (size2[i] - tmp1[i]) / halfaxis[i] * mul;
if (e1 > 0) if (e1 > 0)
if (e1 < secondpos) if (e1 < secondpos)
@@ -605,7 +560,7 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
if (e1 > 0) if (e1 > 0)
if (e1 < secondpos) if (e1 < secondpos)
secondpos = e1; secondpos = e1;
}
} }
secondpos *= mul; secondpos *= mul;
} }
@@ -620,16 +575,15 @@ skip:
mju_addTo3(tmp2, pos2); mju_addTo3(tmp2, pos2);
// collide with // 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_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation
mju_rotVecMat(tmp2, tmp1, mat2); mju_rotVecMat(tmp2, tmp1, mat2);
mju_addTo3(tmp2, pos2); 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; 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* pos1 = D->geom_xpos + 3 * g1;
const mjtNum* mat1 = D->geom_xmat + 9 * 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], 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]; 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 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; int i0, i1, i2;
mjtNum f0, f1, f2; 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; n = 0;
code = -1; code = -1;
mindist2 = mindist * mindist; margin2 = margin * margin;
mju_sub3(tmp1, pos2, pos1); mju_sub3(tmp1, pos2, pos1);
mju_rotVecMatT(pos21, tmp1, mat1); 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_rotVecMat(plen2, size2, rotabs);
mju_rotVecMatT(plen1, size1, 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; 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]; c1 = -fabs(pos21[i]) + size1[i] + plen2[i];
c2 = -fabs(pos12[i]) + size2[i] + plen1[i]; c2 = -fabs(pos12[i]) + size2[i] + plen1[i];
if (c1 < -mindist || c2 < -mindist) if (c1 < -margin || c2 < -margin)
return 0; return 0;
if (c1 < penetration) if (c1 < penetration) {
{
penetration = c1; penetration = c1;
code = i + 3 * (pos21[i] < 0) + 0; code = i + 3 * (pos21[i] < 0) + 0;
} }
if (c2 < penetration) if (c2 < penetration) {
{
penetration = c2; penetration = c2;
code = i + 3 * (pos12[i] < 0) + 6; 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); // printf("%24.16e %24.16e %d %24.16e %d \n",c1,c2,i,penetration,code);
} }
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++) {
{
mju_zero3(tmp2); mju_zero3(tmp2);
if (i == 0) if (i == 0) {
{
tmp2[1] = -rott[3 * j + 2]; tmp2[1] = -rott[3 * j + 2];
tmp2[2] = +rott[3 * j + 1]; tmp2[2] = +rott[3 * j + 1];
} } else if (i == 1) {
else if (i == 1)
{
tmp2[0] = +rott[3 * j + 2]; tmp2[0] = +rott[3 * j + 2];
tmp2[2] = -rott[3 * j + 0]; tmp2[2] = -rott[3 * j + 0];
} } else if (i == 2) {
else if (i == 2)
{
tmp2[0] = -rott[3 * j + 1]; tmp2[0] = -rott[3 * j + 1];
tmp2[1] = +rott[3 * j + 0]; 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); c3 -= fabs(c2);
if (c3 < -mindist) if (c3 < -margin)
return 0; 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); // 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; i2 = 2;
f0 = f1 = f2 = 1; f0 = f1 = f2 = 1;
if (q1 == 0) if (q1 == 0) {
{
i0 = 2; i0 = 2;
f0 = -1; f0 = -1;
i2 = 0; i2 = 0;
} } else if (q1 == 1) {
else if (q1 == 1)
{
i1 = 2; i1 = 2;
f1 = -1; f1 = -1;
i2 = 1; i2 = 1;
} } else if (q1 == 2) {
else if (q1 == 2) {} } else if (q1 == 3) {
else if (q1 == 3)
{
i0 = 2; i0 = 2;
i2 = 0; i2 = 0;
f2 = -1; f2 = -1;
} } else if (q1 == 4) {
else if (q1 == 4)
{
i1 = 2; i1 = 2;
i2 = 1; i2 = 1;
f2 = -1; f2 = -1;
} } else if (q1 == 5) {
else if (q1 == 5)
{
f0 = -1; f0 = -1;
f2 = -1; f2 = -1;
} }
#define rotaxis(vecres,vecin) { vecres[0]=vecin[i0]*f0; vecres[1]=vecin[i1]*f1; vecres[2]=vecin[i2]*f2; } #define rotaxis(vecres, vecin) \
#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); } { \
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_mulMatMatT(r, rotmore, rot, 3, 3, 3);
// mju_rotVecMat(p,pos12,rotmore); // 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); rotaxis(tmp1, size2);
mju_copy3(s, size1); mju_copy3(s, size1);
} } else {
else
{
// mju_mulMatMat(r,rotmore,rot,3,3,3); // mju_mulMatMat(r,rotmore,rot,3,3,3);
rotmatx(r, rot); 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]); 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]; a = lines[i][0 + q];
b = lines[i][3 + q]; b = lines[i][3 + q];
c = lines[i][1 - q]; c = lines[i][1 - q];
d = lines[i][4 - q]; d = lines[i][4 - q];
if (fabs(b) > mjMINVAL) if (fabs(b) > mjMINVAL) {
for (j = -1; j <= 1; j += 2) for (j = -1; j <= 1; j += 2) {
{
l = ss[q] * j; l = ss[q] * j;
c1 = (l - a) * (1 / b); c1 = (l - a) * (1 / b);
if (c1 < 0 || c1 > 1) 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_copy3(points[n], lines[i]);
mju_addToScl3(points[n++], lines[i] + 3, c1); 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; c1 = a * d - b * c;
if (m > 2) if (m > 2) {
for (i = 0; i < 4; i++) for (i = 0; i < 4; i++) {
{
llx = i / 2 ? lx : -lx; llx = i / 2 ? lx : -lx;
lly = i % 2 ? ly : -ly; 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][1] = lly;
points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]); points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]);
n++; 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]); 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; continue;
mju_copy3(points[n++], tmp1); 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++) for (i = 0; i < m; i++)
{ {
if (points[i][2] > mindist) if (points[i][2] > margin)
continue; continue;
mju_copy3(points[n], points[i]); 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 %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)); // 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; ax1 = ax2;
ax2 = 3 - q2 - ax1; ax2 = 3 - q2 - ax1;
} }
if (rottabs[3 * q2 + pax1] < rottabs[3 * q2 + pax2]) if (rottabs[3 * q2 + pax1] < rottabs[3 * q2 + pax2]) {
{
pax1 = pax2; pax1 = pax2;
pax2 = 3 - q1 - pax1; pax2 = 3 - q1 - pax1;
} }
@@ -1079,7 +1014,8 @@ edgeedge:
clface = pax2 + 3; 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); mju_zero(rotmore, 9);
@@ -1102,41 +1038,28 @@ edgeedge:
i2 = 2; i2 = 2;
f0 = f1 = f2 = 1; f0 = f1 = f2 = 1;
if (clface == 0) if (clface == 0) {
{
i0 = 2; i0 = 2;
f0 = -1; f0 = -1;
i2 = 0; i2 = 0;
} } else if (clface == 1) {
else if (clface == 1)
{
i1 = 2; i1 = 2;
f1 = -1; f1 = -1;
i2 = 1; i2 = 1;
} } else if (clface == 2) {
else if (clface == 2) {} } else if (clface == 3) {
else if (clface == 3)
{
i0 = 2; i0 = 2;
i2 = 0; i2 = 0;
f2 = -1; f2 = -1;
} } else if (clface == 4) {
else if (clface == 4)
{
i1 = 2; i1 = 2;
i2 = 1; i2 = 1;
f2 = -1; f2 = -1;
} } else if (clface == 5) {
else if (clface == 5)
{
f0 = -1; f0 = -1;
f2 = -1; f2 = -1;
} }
// mju_rotVecMat(p,pos21,rotmore); // mju_rotVecMat(p,pos21,rotmore);
// mju_rotVecMat(rnorm,clnorm,rotmore); // mju_rotVecMat(rnorm,clnorm,rotmore);
rotaxis(p, pos21); rotaxis(p, pos21);
@@ -1204,7 +1127,6 @@ edgeedge:
// ppts[i][1]=points[i][1]; // ppts[i][1]=points[i][1];
ppts2[i][0] = points[i][0]; ppts2[i][0] = points[i][0];
ppts2[i][1] = points[i][1]; ppts2[i][1] = points[i][1];
} }
@@ -1217,15 +1139,13 @@ edgeedge:
n = 0; n = 0;
if (m > 1) if (m > 1) {
{
mju_copy3(lines[k] + 0, pts[0]); mju_copy3(lines[k] + 0, pts[0]);
mju_copy3(lines[k] + 3, pts[1]); mju_copy3(lines[k] + 3, pts[1]);
mju_copy3(linesu[k] + 0, axi[0]); mju_copy3(linesu[k] + 0, axi[0]);
mju_copy3(linesu[k++] + 3, axi[1]); mju_copy3(linesu[k++] + 3, axi[1]);
} }
if (m > 2) if (m > 2) {
{
mju_copy3(lines[k] + 0, pts[0]); mju_copy3(lines[k] + 0, pts[0]);
mju_copy3(lines[k] + 3, pts[2]); mju_copy3(lines[k] + 3, pts[2]);
mju_copy3(linesu[k] + 0, axi[0]); mju_copy3(linesu[k] + 0, axi[0]);
@@ -1242,19 +1162,16 @@ edgeedge:
mju_copy3(linesu[k++] + 3, axi[1]); mju_copy3(linesu[k++] + 3, axi[1]);
} }
for (i = 0; i < k; i++) for (i = 0; i < k; i++) {
for (q = 0; q < 2; q++) for (q = 0; q < 2; q++) {
{
a = lines[i][0 + q]; a = lines[i][0 + q];
b = lines[i][3 + q]; b = lines[i][3 + q];
c = lines[i][1 - q]; c = lines[i][1 - q];
d = lines[i][4 - q]; d = lines[i][4 - q];
if (fabs(b) > mjMINVAL) {
if (fabs(b) > mjMINVAL) for (j = -1; j <= 1; j += 2) {
for (j = -1; j <= 1; j += 2) if (n < mjMAXCONPAIR) {
if (n < mjMAXCONPAIR)
{
l = s[q] * j; l = s[q] * j;
c1 = (l - a) * (1 / b); c1 = (l - a) * (1 / b);
if (c1 < 0 || c1 > 1) if (c1 < 0 || c1 > 1)
@@ -1263,7 +1180,7 @@ edgeedge:
if (fabs(c2) > s[1 - q]) if (fabs(c2) > s[1 - q])
continue; continue;
if ((linesu[i][2] + linesu[i][5]*c1)*innorm > mindist) if ((linesu[i][2] + linesu[i][5]*c1)*innorm > margin)
continue; continue;
mju_scl3(points[n], linesu[i], 0.5); mju_scl3(points[n], linesu[i], 0.5);
@@ -1272,9 +1189,10 @@ edgeedge:
points[n][1 - q] += 0.5 * c2; points[n][1 - q] += 0.5 * c2;
depth[n] = points[n][2] * innorm * 2; depth[n] = points[n][2] * innorm * 2;
n++; n++;
} }
}
}
}
} }
nl = n; nl = n;
@@ -1285,11 +1203,8 @@ edgeedge:
d = pts[2][1]; d = pts[2][1];
c1 = a * d - b * c; 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; llx = i / 2 ? lx : -lx;
lly = i % 2 ? ly : -ly; lly = i % 2 ? ly : -ly;
@@ -1299,15 +1214,10 @@ edgeedge:
u = (x * d - y * b) * (1 / c1); u = (x * d - y * b) * (1 / c1);
v = (y * a - x * c) * (1 / c1); v = (y * a - x * c) * (1 / c1);
if (nl == 0) {
if (nl == 0)
{
if ((u < 0 || u > 1) && (v < 0 || v > 1)) if ((u < 0 || u > 1) && (v < 0 || v > 1))
continue; continue;
} } else {
else
{
if ((u < 0 || u > 1 || v < 0 || v > 1)) if ((u < 0 || u > 1 || v < 0 || v > 1))
continue; continue;
} }
@@ -1334,37 +1244,32 @@ edgeedge:
c1 = mju_dot3(tmp2, tmp2); c1 = mju_dot3(tmp2, tmp2);
if (tmp1[2] > 0) if (tmp1[2] > 0)
if (c1 > mindist2) if (c1 > margin2)
continue; continue;
mju_add3(points[n], points[n], tmp1); mju_add3(points[n], points[n], tmp1);
mju_scl3(points[n], points[n], 0.5); 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++; n++;
}
} }
nf = n; nf = n;
for (i = 0; i < 4; i++) for (i = 0; i < 4; i++) {
if (n < mjMAXCONPAIR) if (n < mjMAXCONPAIR) {
{
x = ppts2[i][0]; x = ppts2[i][0];
y = ppts2[i][1]; y = ppts2[i][1];
if (nl == 0) {
if (nl == 0) if (nf == 0) {
if (nf == 0) {} } else {
else
{
if (x < -lx || x > lx) if (x < -lx || x > lx)
if (y < -ly || y > ly) if (y < -ly || y > ly)
continue; continue;
} }
else } else {
{
if (x < -lx || x > lx || y < -ly || y > ly) if (x < -lx || x > lx || y < -ly || y > ly)
continue; continue;
} }
@@ -1378,7 +1283,7 @@ edgeedge:
c1 += pu[i][2] * innorm * pu[i][2] * innorm; c1 += pu[i][2] * innorm * pu[i][2] * innorm;
if (pu[i][2] > 0) if (pu[i][2] > 0)
if (c1 > mindist2) if (c1 > margin2)
continue; continue;
@@ -1386,17 +1291,18 @@ edgeedge:
tmp1[1] = ppts2[i][1] * 0.5; tmp1[1] = ppts2[i][1] * 0.5;
tmp1[2] = 0; tmp1[2] = 0;
for (j = 0; j < 2; j++) for (j = 0; j < 2; j++) {
if (ppts2[i][j] < -s[j]) if (ppts2[i][j] < -s[j])
tmp1[j] = -s[j] * 0.5; tmp1[j] = -s[j] * 0.5;
else if (ppts2[i][j] > s[j]) else if (ppts2[i][j] > s[j])
tmp1[j] = +s[j] * 0.5; tmp1[j] = +s[j] * 0.5;
}
mju_addToScl3(tmp1, pu[i], 0.5); mju_addToScl3(tmp1, pu[i], 0.5);
mju_copy3(points[n], tmp1); mju_copy3(points[n], tmp1);
depth[n] = sqrt(c1) * (pu[i][2] < 0 ? -1 : 1); depth[n] = sqrt(c1) * (pu[i][2] < 0 ? -1 : 1);
n++; n++;
}
} }
mju_mulMatMatT(r, mat1, rotmore, 3, 3, 3); mju_mulMatMatT(r, mat1, rotmore, 3, 3, 3);
@@ -1407,8 +1313,7 @@ edgeedge:
mju_zero3(con[0].frame + 3); mju_zero3(con[0].frame + 3);
for (i = 0; i < n; i++) for (i = 0; i < n; i++) {
{
con[i].dist = depth[i]; con[i].dist = depth[i];
points[i][2] += hz; points[i][2] += hz;
@@ -1417,11 +1322,8 @@ edgeedge:
mju_add3(con[i].pos, tmp2, pos1); mju_add3(con[i].pos, tmp2, pos1);
mju_copy(con[i].frame, con[0].frame, 6); mju_copy(con[i].frame, con[0].frame, 6);
} }
return n; return n;
#undef rotaxis #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 // copy best vertex
else else {
for (int i=0; i<3; i++) { for (int i=0; i<3; i++) {
res[i] = (mjtNum)vertdata[3*ibest + i]; res[i] = (mjtNum)vertdata[3*ibest + i];
} }
}
break; break;
default: 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 (?) 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[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE
&& m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) { && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) {
// multiCCD parameters // multiCCD parameters
const mjtNum relative_tolerance = 1e-3; const mjtNum relative_tolerance = 1e-3;
const mjtNum perturbation_angle = 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 // table of pair-wise collision functions
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH */ /* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH */
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex}, /*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}, /*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}, /*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex},
/*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex}, /*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex},
/*MESH */ {0, 0, 0, 0, 0, 0, 0, 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 // finish merging predefined pairs
if (npair && m->opt.collision==mjCOL_ALL) if (npair && m->opt.collision==mjCOL_ALL) {
while (pairadr<npair) { while (pairadr<npair) {
mj_collideGeoms(m, d, pairadr++, -1, 0, 0); mj_collideGeoms(m, d, pairadr++, -1, 0, 0);
} }
} }
}
mjFREESTACK; 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 // dense: copy entire Jacobian
if (!mj_isSparse(m)) { if (!mj_isSparse(m)) {
// make sure jac is not empty // make sure jac is not empty
if (empty) if (empty) {
for (int i=0; i<size*nv; i++) for (int i=0; i<size*nv; i++) {
if (jac[i]) { if (jac[i]) {
empty = 0; empty = 0;
break; break;
} }
}
}
// copy if not empty // copy if not empty
if (!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; deriv = data[1] + 2*data[2]*dif + 3*data[3]*dif*dif + 4*data[4]*dif*dif*dif;
// compute Jacobian: sparse or dense // compute Jacobian: sparse or dense
if (issparse) if (issparse) {
NV = mju_combineSparse(jac[0], jac[1], nv, 1, -deriv, NV = mju_combineSparse(jac[0], jac[1], nv, 1, -deriv,
NV, NV2, chain, chain2, NV, NV2, chain, chain2,
sparse_buf, buf_ind); sparse_buf, buf_ind);
else { } else {
mju_addToScl(jac[0], jac[1], -deriv, nv); 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); int size, int type, int id, int NV, const int* chain);
// merge dof chains for two bodies // 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 // 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 ---------------------------------------------- //-------------------------- 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 // map inertias and motion dofs to global frame centered at subtree-CoM
void mj_comPos(const mjModel* m, mjData* d) { void mj_comPos(const mjModel* m, mjData* d) {
mjtNum offset[3], axis[3]; mjtNum offset[3], axis[3];
mjMARKSTACK; mjMARKSTACK;
mjtNum* mass_subtree = mj_stackAlloc(d, m->nbody); mjtNum* mass_subtree = mj_stackAlloc(d, m->nbody);
@@ -1798,7 +1797,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 1 // body 1
if ((k = m->eq_obj1id[id])) { if ((k = m->eq_obj1id[id])) {
// transform connect point on body1: local -> global // transform connect point on body1: local -> global
mju_rotVecMat(pos, eq_data, d->xmat+9*k); mju_rotVecMat(pos, eq_data, d->xmat+9*k);
mju_addTo3(pos, d->xpos+3*k); mju_addTo3(pos, d->xpos+3*k);
@@ -1812,7 +1810,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 2 // body 2
if ((k = m->eq_obj2id[id])) { if ((k = m->eq_obj2id[id])) {
// transform connect point on body2: local -> global // transform connect point on body2: local -> global
mju_rotVecMat(pos, eq_data + 3, d->xmat+9*k); mju_rotVecMat(pos, eq_data + 3, d->xmat+9*k);
mju_addTo3(pos, d->xpos+3*k); mju_addTo3(pos, d->xpos+3*k);
@@ -1836,7 +1833,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 1 // body 1
if ((k = m->eq_obj1id[id])) { if ((k = m->eq_obj1id[id])) {
// transform weld point on body1: local -> global // transform weld point on body1: local -> global
mju_rotVecMat(pos, eq_data, d->xmat+9*k); mju_rotVecMat(pos, eq_data, d->xmat+9*k);
mju_addTo3(pos, d->xpos+3*k); mju_addTo3(pos, d->xpos+3*k);
@@ -1850,7 +1846,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 2 // body 2
if ((k = m->eq_obj2id[id])) { if ((k = m->eq_obj2id[id])) {
// weld force on body2 is always applied at body root // weld force on body2 is always applied at body root
mju_copy3(pos, d->xpos+3*k); 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 i=0; i<n; i++) {
for (int j=0; j<n; j++) { for (int j=0; j<n; j++) {
if (B[i*n+j]) { if (B[i*n+j]) {
// process non-zero elements of J(i,k) // process non-zero elements of J(i,k)
for (int k=0; k<rownnz[offset+i]; k++) { for (int k=0; k<rownnz[offset+i]; k++) {
int ik = rowadr[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, mjtNum* restrict B, const mjtNum local_vels[6], const mjtNum fluid_density,
const mjtNum virtual_mass[3], const mjtNum virtual_inertia[3]) { 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 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] = { const mjtNum virtual_lin_mom[3] = {
fluid_density * virtual_mass[0] * lin_vel[0], fluid_density * virtual_mass[0] * lin_vel[0],
fluid_density * virtual_mass[1] * lin_vel[1], 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 // finite-difference activations: skip=mjSTAGE_VEL
if (DyDa || DsDa) { if (DyDa || DsDa) {
for (int i=0; i<na; i++) { for (int i=0; i<na; i++) {
// nudge forward // nudge forward
d->act[i] += eps; 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 // ensure file size fits in int
long long_filesize = ftell(fp); long long_filesize = ftell(fp); // NOLINT(runtime/int)
if (long_filesize > INT_MAX) { if (long_filesize > INT_MAX) {
fclose(fp); fclose(fp);
mju_warning_s("File size over 2GB is not supported. File: '%s'", filename); 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 // Euler integrator, semi-implicit in velocity, possibly skipping factorisation
void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
int i, nv = m->nv, nM = m->nM; int i, nv = m->nv, nM = m->nM;
mjMARKSTACK; mjMARKSTACK;
mjtNum* qfrc = mj_stackAlloc(d, nv); 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 // validate reference fields in a model; return null if valid, error message otherwise
const char* mj_validateReferences(const mjModel* m) { const char* mj_validateReferences(const mjModel* m) {
// for each field in mjModel that refers to another field, call X with: // for each field in mjModel that refers to another field, call X with:
// adrarray: array containing the references // adrarray: array containing the references
// nadrs: number of elements in refarray // 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_ #ifndef MUJOCO_SRC_ENGINE_ENGINE_MACRO_H_
#define 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 -------------------------------------------------- //-------------------------------- 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 // real or positive: add noise directly, with clamp for positive
if (m->sensor_datatype[i]==mjDATATYPE_REAL || 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++) { for (int j=0; j<dim; j++) {
// get random numbers; use only the first one // get random numbers; use only the first one
rnd[0] = mju_standardNormal(rnd+1); 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; d->sensordata[adr+j] += rnd[0]*noise;
} }
} }
}
// axis or quat: rotate around random axis by random angle // axis or quat: rotate around random axis by random angle
else { else {
@@ -114,7 +115,6 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
// process all dimensions // process all dimensions
for (int j=0; j<dim; j++) { for (int j=0; j<dim; j++) {
// real: apply on both sides // real: apply on both sides
if (m->sensor_datatype[i]==mjDATATYPE_REAL) { if (m->sensor_datatype[i]==mjDATATYPE_REAL) {
d->sensordata[adr+j] = mju_clip(d->sensordata[adr+j], -cutoff, cutoff); 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 // write datetime, type: message to MUJOCO_LOG.TXT
void mju_writeLog(const char* type, const char* msg) { void mju_writeLog(const char* type, const char* msg) {
time_t rawtime; time_t rawtime;
struct tm *timeinfo; struct tm timeinfo;
FILE* fp = fopen("MUJOCO_LOG.TXT", "a+t"); FILE* fp = fopen("MUJOCO_LOG.TXT", "a+t");
if (fp) { if (fp) {
// get time // get time
time(&rawtime); 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 // 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); fclose(fp);
} }
} }
@@ -151,7 +160,7 @@ void mju_warning(const char* msg) {
// error with int argument // error with int argument
void mju_error_i(const char* msg, int i) { void mju_error_i(const char* msg, int i) {
char errmsg[1000]; char errmsg[1000];
snprintf(errmsg, 1000, msg, i); snprintf(errmsg, sizeof(errmsg), msg, i);
errmsg[999] = '\0'; errmsg[999] = '\0';
mju_error(errmsg); mju_error(errmsg);
} }
@@ -160,7 +169,7 @@ void mju_error_i(const char* msg, int i) {
// warning with int argument // warning with int argument
void mju_warning_i(const char* msg, int i) { void mju_warning_i(const char* msg, int i) {
char wrnmsg[1000]; char wrnmsg[1000];
snprintf(wrnmsg, 1000, msg, i); snprintf(wrnmsg, sizeof(wrnmsg), msg, i);
wrnmsg[999] = '\0'; wrnmsg[999] = '\0';
mju_warning(wrnmsg); mju_warning(wrnmsg);
} }
@@ -169,7 +178,7 @@ void mju_warning_i(const char* msg, int i) {
// error string argument // error string argument
void mju_error_s(const char* msg, const char* text) { void mju_error_s(const char* msg, const char* text) {
char errmsg[1000]; char errmsg[1000];
snprintf(errmsg, 1000, msg, text); snprintf(errmsg, sizeof(errmsg), msg, text);
errmsg[999] = '\0'; errmsg[999] = '\0';
mju_error(errmsg); mju_error(errmsg);
} }
@@ -178,7 +187,7 @@ void mju_error_s(const char* msg, const char* text) {
// warning string argument // warning string argument
void mju_warning_s(const char* msg, const char* text) { void mju_warning_s(const char* msg, const char* text) {
char wrnmsg[1000]; char wrnmsg[1000];
snprintf(wrnmsg, 1000, msg, text); snprintf(wrnmsg, sizeof(wrnmsg), msg, text);
wrnmsg[999] = '\0'; wrnmsg[999] = '\0';
mju_warning(wrnmsg); mju_warning(wrnmsg);
} }
src/engine/engine_util_solve.c
+4 -3
View File
@@ -18,7 +18,6 @@
#include <string.h> #include <string.h>
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_io.h" #include "engine/engine_io.h"
#include "engine/engine_macro.h" #include "engine/engine_macro.h"
#include "engine/engine_util_blas.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; mat[k*(n+1)] = r;
// update mat // update mat
if (flg_plus) if (flg_plus) {
for (int i=k+1; i<n; i++) { for (int i=k+1; i<n; i++) {
mat[i*n+k] = (mat[i*n+k] + s*x[i])*cinv; mat[i*n+k] = (mat[i*n+k] + s*x[i])*cinv;
} else }
} else {
for (int i=k+1; i<n; i++) { for (int i=k+1; i<n; i++) {
mat[i*n+k] = (mat[i*n+k] - s*x[i])*cinv; mat[i*n+k] = (mat[i*n+k] - s*x[i])*cinv;
} }
}
// update x // update x
for (int i=k+1; i<n; i++) { 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) { const int* colind, const int* rowsuper) {
// no supernodes, or no AVX // no supernodes, or no AVX
#ifdef mjUSEAVX #ifdef mjUSEAVX
if (!rowsuper) if (!rowsuper) {
#endif #endif
{
// regular sparse dot-product // regular sparse dot-product
for (int r=0; r<nr; r++) { for (int r=0; r<nr; r++) {
res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]); res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
} }
return; return;
#ifdef mjUSEAVX
} }
#endif
// regular or supernode // regular or supernode
for (int r=0; r<nr; r++) { 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 --------------------------------------- //----------------------------- 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 // 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 // return if there is no space in buffer
#define START \ #define START \
if( scn->ngeom>=scn->maxgeom ) \ if ( scn->ngeom>=scn->maxgeom ) { \
{ mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); \ mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); \
return; } \ return; \
else { thisgeom = scn->geoms + scn->ngeom; \ } else { \
thisgeom = scn->geoms + scn->ngeom; \
mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); \ mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); \
thisgeom->objtype = objtype; \ thisgeom->objtype = objtype; \
thisgeom->objid = i; \ thisgeom->objid = i; \
thisgeom->category = category; \ thisgeom->category = category; \
thisgeom->segid = scn->ngeom; } thisgeom->segid = scn->ngeom; \
}
// advance counter // advance counter
@@ -327,7 +331,7 @@ void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size,
geom->type = type; geom->type = type;
// set size (for XYZ scaling) // set size (for XYZ scaling)
if (size) if (size) {
switch (type) { switch (type) {
case mjGEOM_SPHERE: case mjGEOM_SPHERE:
geom->size[0] = (float)size[0]; geom->size[0] = (float)size[0];
@@ -349,6 +353,7 @@ void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size,
default: default:
mju_n2f(geom->size, size, 3); mju_n2f(geom->size, size, 3);
}
} else { } else {
geom->size[0] = 0.1f; geom->size[0] = 0.1f;
geom->size[1] = 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 || if (m->actuator_trntype[i]==mjTRN_JOINT ||
m->actuator_trntype[i]==mjTRN_JOINTINPARENT || m->actuator_trntype[i]==mjTRN_JOINTINPARENT ||
m->actuator_trntype[i]==mjTRN_SITE) { m->actuator_trntype[i]==mjTRN_SITE) {
START START
// site actuators // site actuators
@@ -1355,8 +1359,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// slider-crank // slider-crank
objtype = mjOBJ_ACTUATOR; objtype = mjOBJ_ACTUATOR;
category = mjCAT_DYNAMIC; category = mjCAT_DYNAMIC;
if ((category & catmask)) if ((category & catmask)) {
for (int i=0; i<m->nu; i++) for (int i=0; i<m->nu; i++) {
if (m->actuator_trntype[i]==mjTRN_SLIDERCRANK) { if (m->actuator_trntype[i]==mjTRN_SLIDERCRANK) {
// get data // get data
int j = m->actuator_trnid[2*i]; // crank int j = m->actuator_trnid[2*i]; // crank
@@ -1404,6 +1408,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
} }
FINISH FINISH
} }
}
}
// center of mass for root bodies // center of mass for root bodies
objtype = mjOBJ_UNKNOWN; objtype = mjOBJ_UNKNOWN;
@@ -1778,7 +1784,6 @@ void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
// update visible skins only // update visible skins only
void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) { void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
// process skins // process skins
for (int i=0; i<m->nskin; i++) { for (int i=0; i<m->nskin; i++) {
// get info // get info
src/user/user_util.cc
+1
View File
@@ -20,6 +20,7 @@
#include <limits> #include <limits>
#include <string> #include <string>
#include <mujoco/mjtnum.h>
#include "engine/engine_macro.h" #include "engine/engine_macro.h"
using std::isnan; using std::isnan;