From 600f0f20bceafce753a89c95f40094dcc296c6fc Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 12 Dec 2025 04:48:21 -0800 Subject: [PATCH] Introduce private header `engine_inline.h` exploiting `restrict` and avoiding loops and copies in some commonly used utility functions. PiperOrigin-RevId: 843635464 Change-Id: I3b0553eea98424ccc7def77e3769e2994f3e9014 --- src/engine/CMakeLists.txt | 1 + src/engine/engine_collision_box.c | 235 ++++++------ src/engine/engine_collision_convex.c | 165 +++++---- src/engine/engine_collision_primitive.c | 138 +++---- src/engine/engine_core_smooth.c | 220 +++++------ src/engine/engine_core_util.c | 40 +- src/engine/engine_inline.h | 464 ++++++++++++++++++++++++ src/engine/engine_passive.c | 83 ++--- src/engine/engine_support.c | 1 + src/engine/engine_util_solve.c | 5 +- src/engine/engine_util_spatial.c | 99 ++--- 11 files changed, 964 insertions(+), 487 deletions(-) create mode 100644 src/engine/engine_inline.h diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index f9980eb4..3758ed70 100644 --- a/src/engine/CMakeLists.txt +++ b/src/engine/CMakeLists.txt @@ -46,6 +46,7 @@ set(MUJOCO_ENGINE_SRCS engine_inverse.h engine_init.c engine_init.h + engine_inline.h engine_island.c engine_island.h engine_io.c diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index 866f4769..ace8de96 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -15,6 +15,7 @@ #include #include "engine/engine_collision_primitive.h" +#include "engine/engine_inline.h" #include "engine/engine_util_blas.h" // hard-clamp vector to range [-limit(i), +limit(i)] @@ -43,14 +44,14 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin, mjtNum pos[3]; mjtNum dist, closest; - mju_sub3(tmp, pos1, pos2); - mju_mulMatTVec3(center, mat2, tmp); + mji_sub3(tmp, pos1, pos2); + mji_mulMatTVec3(center, mat2, tmp); - mju_copy(clamped, center, 3); + mji_copy3(clamped, center); mju_clampVec(clamped, size2, 3); - mju_copy(deepest, center, 3); - mju_sub3(tmp, clamped, center); + mji_copy3(deepest, center); + mji_sub3(tmp, clamped, center); dist = mju_normalize3(tmp); if (dist - size1[0] > margin) @@ -70,20 +71,20 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin, mjtNum nearest[3] = {0}; nearest[k / 2] = (k % 2 ? -1 : 1); - mju_copy3(pos, center); - mju_addToScl3(pos, nearest, (size1[0] - closest) / 2); - mju_mulMatVec3(con[0].frame, mat2, nearest); + mji_copy3(pos, center); + mji_addToScl3(pos, nearest, (size1[0] - closest) / 2); + mji_mulMatVec3(con[0].frame, mat2, nearest); dist = -closest; } else { - mju_addToScl3(deepest, tmp, size1[0]); + mji_addToScl3(deepest, tmp, size1[0]); mju_zero3(pos); - mju_addToScl3(pos, clamped, 0.5); - mju_addToScl3(pos, deepest, 0.5); - mju_mulMatVec3(con[0].frame, mat2, tmp); + mji_addToScl3(pos, clamped, 0.5); + mji_addToScl3(pos, deepest, 0.5); + mji_mulMatVec3(con[0].frame, mat2, tmp); } - mju_mulMatVec3(tmp, mat2, pos); - mju_add3(con[0].pos, tmp, pos2); + mji_mulMatVec3(tmp, mat2, pos); + mji_add3(con[0].pos, tmp, pos2); con[0].dist = dist - size1[0]; mju_zero3(con[0].frame + 3); @@ -147,15 +148,15 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, 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_mulMatTVec3(pos, mat2, tmp1); // and axis parralel to world + mji_sub3(tmp1, pos1, pos2); // bring capsule to box-local frame (center's box is at (0,0,0)) + mji_mulMatTVec3(pos, mat2, tmp1); // and axis parralel to world tmp1[0] = mat1[2]; // capsule's axis tmp1[1] = mat1[5]; tmp1[2] = mat1[8]; - mju_mulMatTVec3(axis, mat2, tmp1); // do the same for the capsule axis - mju_scl3(halfaxis, axis, halflength); // scale to get actual capsule half-axis + mji_mulMatTVec3(axis, mat2, tmp1); // do the same for the capsule axis + mji_scl3(halfaxis, axis, halflength); // scale to get actual capsule half-axis axisdir = 0; if (halfaxis[0] > 0) @@ -177,9 +178,9 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, // test to see if maybe the a face of the box is closest to the capsule for (i = -1; i <= 1; i += 2) { - mju_copy3(tmp1, pos); - mju_addToScl3(tmp1, halfaxis, i); - mju_copy3(tmp2, tmp1); + mji_copy3(tmp1, pos); + mji_addToScl3(tmp1, halfaxis, i); + mji_copy3(tmp2, tmp1); for (c1 = 0, j = 0, c2 = -1; j < 3; j++) { if (tmp1[j] < -size2[j]) { @@ -196,7 +197,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, if (c1 > 1) continue; - mju_subFrom3(tmp1, tmp2); + mji_subFrom3(tmp1, tmp2); dist = mju_dot3(tmp1, tmp1); if (dist < bestdist) { @@ -225,7 +226,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, // find closest point between capsule and the edge - mju_sub3(dif, tmp3, pos); + mji_sub3(dif, tmp3, pos); ma = size2[j] * size2[j]; mb = -size2[j] * halfaxis[j]; @@ -275,9 +276,9 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, x1 = -1, s1 = 0; } - mju_sub3(dif, tmp3, pos); + mji_sub3(dif, tmp3, pos); - mju_addToScl3(dif, halfaxis, -x2); + mji_addToScl3(dif, halfaxis, -x2); dif[j] += size2[j] * x1; tmp1[2] = mju_dot3(dif, dif); @@ -546,8 +547,8 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, secondpos = 2; - mju_copy3(tmp1, pos); - mju_addToScl3(tmp1, halfaxis, -mul); + mji_copy3(tmp1, pos); + mji_addToScl3(tmp1, halfaxis, -mul); for (i = 0; i < 3; i++) { if (i != clface) { @@ -570,7 +571,7 @@ skip: // create sphere in original orientation at first contact point mju_copy3(tmp1, pos); - mju_addToScl3(tmp1, halfaxis, bestsegmentpos); + mji_addToScl3(tmp1, halfaxis, bestsegmentpos); mju_mulMatVec3(tmp2, mat2, tmp1); mju_addTo3(tmp2, pos2); @@ -580,7 +581,7 @@ skip: if (secondpos > -3) { // secondpos was modified mju_copy3(tmp1, pos); - mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation + mji_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation mju_mulMatVec3(tmp2, mat2, tmp1); mju_addTo3(tmp2, pos2); n += mjraw_SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2); @@ -626,11 +627,11 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 code = -1; margin2 = margin * margin; - mju_sub3(tmp1, pos2, pos1); - mju_mulMatTVec3(pos21, mat1, tmp1); + mji_sub3(tmp1, pos2, pos1); + mji_mulMatTVec3(pos21, mat1, tmp1); - mju_sub3(tmp1, pos1, pos2); - mju_mulMatTVec3(pos12, mat2, tmp1); + mji_sub3(tmp1, pos1, pos2); + mji_mulMatTVec3(pos12, mat2, tmp1); mju_mulMatTMat3(rot, mat1, mat2); mju_transpose(rott, rot, 3, 3); @@ -640,8 +641,8 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 for (i = 0; i < 9; i++) rottabs[i] = mju_abs(rott[i]); - mju_mulMatVec3(plen2, rotabs, size2); - mju_mulMatTVec3(plen1, rotabs, size1); + mji_mulMatVec3(plen2, rotabs, size2); + mji_mulMatTVec3(plen1, rotabs, size1); for (i = 0, penetration = margin; i < 3; i++) penetration += size1[i] * 3 + size2[i] * 3; @@ -716,7 +717,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 cle2 += 1 << k; code = 12 + i * 3 + j; - mju_copy3(clnorm, tmp2); + mji_copy3(clnorm, tmp2); in = c2 < 0; } @@ -792,9 +793,9 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 } #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); \ + mji_scl3(matres+0, matin+i0*3, f0); \ + mji_scl3(matres+3, matin+i1*3, f1); \ + mji_scl3(matres+6, matin+i2*3, f2); \ } if (q2) { @@ -806,7 +807,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 rotaxis(p, pos12); rotaxis(tmp1, size2); - mju_copy3(s, size1); + mji_copy3(s, size1); } else { // mju_mulMatMat(r,rotmore,rot,3,3,3); @@ -818,7 +819,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 rotaxis(p, pos21); rotaxis(tmp1, size1); - mju_copy3(s, size2); + mji_copy3(s, size2); } mju_transpose(rt, r, 3, 3); @@ -831,40 +832,40 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 hz = ss[2]; p[2] -= hz; - mju_copy3(lp, p); + mji_copy3(lp, p); for (clcorner = 0, i = 0; i < 3; i++) if (r[6 + i] < 0) clcorner += 1 << i; - mju_addToScl3(lp, rt + 0, s[0] * ((clcorner & 1) ? 1 : -1)); - mju_addToScl3(lp, rt + 3, s[1] * ((clcorner & 2) ? 1 : -1)); - mju_addToScl3(lp, rt + 6, s[2] * ((clcorner & 4) ? 1 : -1)); + mji_addToScl3(lp, rt + 0, s[0] * ((clcorner & 1) ? 1 : -1)); + mji_addToScl3(lp, rt + 3, s[1] * ((clcorner & 2) ? 1 : -1)); + mji_addToScl3(lp, rt + 6, s[2] * ((clcorner & 4) ? 1 : -1)); m = k = 0; - mju_copy3(pts[m++], lp); + mji_copy3(pts[m++], lp); for (i = 0; i < 3; i++) if (mju_abs(r[6 + i]) < 0.5) mju_scl3(pts[m++], rt + 3 * i, s[i] * ((clcorner & (1 << i)) ? -2 : 2)); - mju_add3(pts[3], pts[0], pts[1]); - mju_add3(pts[4], pts[0], pts[2]); - mju_add3(pts[5], pts[3], pts[2]); + mji_add3(pts[3], pts[0], pts[1]); + mji_add3(pts[4], pts[0], pts[2]); + mji_add3(pts[5], pts[3], pts[2]); if (m > 1) { - mju_copy3(lines[k] + 0, pts[0]); - mju_copy3(lines[k++] + 3, pts[1]); + mji_copy3(lines[k] + 0, pts[0]); + mji_copy3(lines[k++] + 3, pts[1]); } if (m > 2) { - mju_copy3(lines[k] + 0, pts[0]); - mju_copy3(lines[k++] + 3, pts[2]); - mju_copy3(lines[k] + 0, pts[3]); - mju_copy3(lines[k++] + 3, pts[2]); - mju_copy3(lines[k] + 0, pts[4]); - mju_copy3(lines[k++] + 3, pts[1]); + mji_copy3(lines[k] + 0, pts[0]); + mji_copy3(lines[k++] + 3, pts[2]); + mji_copy3(lines[k] + 0, pts[3]); + mji_copy3(lines[k++] + 3, pts[2]); + mji_copy3(lines[k] + 0, pts[4]); + mji_copy3(lines[k++] + 3, pts[1]); } for (i = 0; i < k; i++) { @@ -884,8 +885,8 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 if (mju_abs(c2) > ss[1 - q]) continue; - mju_copy3(points[n], lines[i]); - mju_addToScl3(points[n++], lines[i] + 3, c1); + mji_copy3(points[n], lines[i]); + mji_addToScl3(points[n++], lines[i] + 3, c1); } } } @@ -920,7 +921,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 } for (i = 0; i < (1 << (m - 1)); i++) { - mju_copy3(tmp1, pts[i == 0 ? 0 : i + 2]); + mji_copy3(tmp1, pts[i == 0 ? 0 : i + 2]); if (i) @@ -930,7 +931,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 if (tmp1[1] <= -ly || tmp1[1] >= ly) continue; - mju_copy3(points[n++], tmp1); + mji_copy3(points[n++], tmp1); } @@ -941,7 +942,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 { if (points[i][2] > margin) continue; - mju_copy3(points[n], points[i]); + mji_copy3(points[n], points[i]); depth[n] = points[n][2]; points[n][2] *= 0.5; @@ -957,7 +958,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 tmp2[1] = (q2 ? -1 : 1) * r[5]; tmp2[2] = (q2 ? -1 : 1) * r[8]; - mju_copy3(con[0].frame, tmp2); + mji_copy3(con[0].frame, tmp2); mju_zero3(con[0].frame + 3); @@ -968,11 +969,11 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 con[i].dist = 2 * points[i][2]; points[i][2] += hz; - mju_mulMatVec3(tmp2, r, points[i]); - mju_add3(con[i].pos, tmp2, p); + mji_mulMatVec3(tmp2, r, points[i]); + mji_add3(con[i].pos, tmp2, p); if (i) - mju_copy(con[i].frame, con[0].frame, 6); + mji_copy6(con[i].frame, con[0].frame); } @@ -1078,7 +1079,7 @@ edgeedge: // mju_mulMatMat(r,rotmore,rot,3,3,3); rotmatx(r, rot); - mju_mulMatTVec3(tmp1, rotmore, size1); + mji_mulMatTVec3(tmp1, rotmore, size1); for (i = 0; i < 3; i++) s[i] = mju_abs(tmp1[i]); @@ -1092,29 +1093,29 @@ edgeedge: n = 0; - mju_copy3(points[n], p); - mju_addToScl3(points[n], rt + 3 * ax1, size2[ax1] * ((cle2 & (1 << ax1)) ? 1 : -1)); - mju_addToScl3(points[n], rt + 3 * ax2, size2[ax2] * ((cle2 & (1 << ax2)) ? 1 : -1)); - mju_copy3(points[n + 1], points[n]); - mju_addToScl3(points[n], rt + 3 * q2, size2[q2]); + mji_copy3(points[n], p); + mji_addToScl3(points[n], rt + 3 * ax1, size2[ax1] * ((cle2 & (1 << ax1)) ? 1 : -1)); + mji_addToScl3(points[n], rt + 3 * ax2, size2[ax2] * ((cle2 & (1 << ax2)) ? 1 : -1)); + mji_copy3(points[n + 1], points[n]); + mji_addToScl3(points[n], rt + 3 * q2, size2[q2]); n = 1; - mju_addToScl3(points[n], rt + 3 * q2, -size2[q2]); + mji_addToScl3(points[n], rt + 3 * q2, -size2[q2]); n = 2; - mju_copy3(points[n], p); - mju_addToScl3(points[n], rt + 3 * ax1, size2[ax1] * ((cle2 & (1 << ax1)) ? -1 : 1)); - mju_addToScl3(points[n], rt + 3 * ax2, size2[ax2] * ((cle2 & (1 << ax2)) ? 1 : -1)); - mju_copy3(points[n + 1], points[n]); - mju_addToScl3(points[n], rt + 3 * q2, size2[q2]); + mji_copy3(points[n], p); + mji_addToScl3(points[n], rt + 3 * ax1, size2[ax1] * ((cle2 & (1 << ax1)) ? -1 : 1)); + mji_addToScl3(points[n], rt + 3 * ax2, size2[ax2] * ((cle2 & (1 << ax2)) ? 1 : -1)); + mji_copy3(points[n + 1], points[n]); + mji_addToScl3(points[n], rt + 3 * q2, size2[q2]); n = 3; - mju_addToScl3(points[n], rt + 3 * q2, -size2[q2]); + mji_addToScl3(points[n], rt + 3 * q2, -size2[q2]); n = 4; - mju_copy3(axi[0], points[0]); - mju_sub3(axi[1], points[1], points[0]); - mju_sub3(axi[2], points[2], points[0]); + mji_copy3(axi[0], points[0]); + mji_sub3(axi[1], points[1], points[0]); + mji_sub3(axi[2], points[2], points[0]); if (mju_abs(rnorm[2]) < mjMINVAL) @@ -1127,9 +1128,9 @@ edgeedge: { c1 = -points[i][2] * (1 / rnorm[2]); - mju_copy3(pu[i], points[i]); + mji_copy3(pu[i], points[i]); - mju_addToScl3(points[i], rnorm, c1); + mji_addToScl3(points[i], rnorm, c1); // ppts[i][0]=points[i][0]; // ppts[i][1]=points[i][1]; @@ -1138,9 +1139,9 @@ edgeedge: } - mju_copy3(pts[0], points[0]); - mju_sub3(pts[1], points[1], points[0]); - mju_sub3(pts[2], points[2], points[0]); + mji_copy3(pts[0], points[0]); + mji_sub3(pts[1], points[1], points[0]); + mji_sub3(pts[2], points[2], points[0]); m = 3; k = 0; @@ -1148,26 +1149,26 @@ edgeedge: 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]); + mji_copy3(lines[k] + 0, pts[0]); + mji_copy3(lines[k] + 3, pts[1]); + mji_copy3(linesu[k] + 0, axi[0]); + mji_copy3(linesu[k++] + 3, axi[1]); } if (m > 2) { - mju_copy3(lines[k] + 0, pts[0]); - mju_copy3(lines[k] + 3, pts[2]); - mju_copy3(linesu[k] + 0, axi[0]); - mju_copy3(linesu[k++] + 3, axi[2]); + mji_copy3(lines[k] + 0, pts[0]); + mji_copy3(lines[k] + 3, pts[2]); + mji_copy3(linesu[k] + 0, axi[0]); + mji_copy3(linesu[k++] + 3, axi[2]); - mju_add3(lines[k] + 0, pts[0], pts[1]); - mju_copy3(lines[k] + 3, pts[2]); - mju_add3(linesu[k] + 0, axi[0], axi[1]); - mju_copy3(linesu[k++] + 3, axi[2]); + mji_add3(lines[k] + 0, pts[0], pts[1]); + mji_copy3(lines[k] + 3, pts[2]); + mji_add3(linesu[k] + 0, axi[0], axi[1]); + mji_copy3(linesu[k++] + 3, axi[2]); - mju_add3(lines[k] + 0, pts[0], pts[2]); - mju_copy3(lines[k] + 3, pts[1]); - mju_add3(linesu[k] + 0, axi[0], axi[2]); - mju_copy3(linesu[k++] + 3, axi[1]); + mji_add3(lines[k] + 0, pts[0], pts[2]); + mji_copy3(lines[k] + 3, pts[1]); + mji_add3(linesu[k] + 0, axi[0], axi[2]); + mji_copy3(linesu[k++] + 3, axi[1]); } for (i = 0; i < k; i++) { @@ -1191,8 +1192,8 @@ edgeedge: if ((linesu[i][2] + linesu[i][5]*c1)*innorm > margin) continue; - mju_scl3(points[n], linesu[i], 0.5); - mju_addToScl3(points[n], linesu[i] + 3, 0.5 * c1); + mji_scl3(points[n], linesu[i], 0.5); + mji_addToScl3(points[n], linesu[i] + 3, 0.5 * c1); points[n][0 + q] += 0.5 * l; points[n][1 - q] += 0.5 * c2; depth[n] = points[n][2] * innorm * 2; @@ -1240,22 +1241,22 @@ edgeedge: v = 1; - mju_scl3(tmp1, pu[0], 1 - u - v); - mju_addToScl3(tmp1, pu[1], u); - mju_addToScl3(tmp1, pu[2], v); + mji_scl3(tmp1, pu[0], 1 - u - v); + mji_addToScl3(tmp1, pu[1], u); + mji_addToScl3(tmp1, pu[2], v); points[n][0] = llx; points[n][1] = lly; points[n][2] = 0; - mju_sub3(tmp2, points[n], tmp1); + mji_sub3(tmp2, points[n], tmp1); c1 = mju_dot3(tmp2, tmp2); if (tmp1[2] > 0) if (c1 > margin2) continue; - mju_add3(points[n], points[n], tmp1); + mji_add3(points[n], points[n], tmp1); mju_scl3(points[n], points[n], 0.5); depth[n] = sqrt(c1) * (tmp1[2] < 0 ? -1 : 1); @@ -1305,8 +1306,8 @@ edgeedge: 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); + mji_addToScl3(tmp1, pu[i], 0.5); + mji_copy3(points[n], tmp1); depth[n] = sqrt(c1) * (pu[i][2] < 0 ? -1 : 1); n++; @@ -1315,9 +1316,9 @@ edgeedge: mju_mulMatMatT3(r, mat1, rotmore); - mju_mulMatVec3(tmp1, r, rnorm); + mji_mulMatVec3(tmp1, r, rnorm); - mju_scl3(con[0].frame, tmp1, in ? -1 : 1); + mji_scl3(con[0].frame, tmp1, in ? -1 : 1); mju_zero3(con[0].frame + 3); @@ -1325,11 +1326,11 @@ edgeedge: con[i].dist = depth[i]; points[i][2] += hz; - mju_mulMatVec3(tmp2, r, points[i]); + mji_mulMatVec3(tmp2, r, points[i]); - mju_add3(con[i].pos, tmp2, pos1); + mji_add3(con[i].pos, tmp2, pos1); - mju_copy(con[i].frame, con[0].frame, 6); + mji_copy6(con[i].frame, con[0].frame); } return n; diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index cfb8d1b1..1a182531 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -26,6 +26,7 @@ #include "engine/engine_collision_gjk.h" #include "engine/engine_collision_primitive.h" #include "engine/engine_memory.h" +#include "engine/engine_inline.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" @@ -99,17 +100,17 @@ void mjc_center(mjtNum res[3], const mjCCDObj *obj) { // return geom position if (g >= 0) { - mju_copy3(res, obj->data->geom_xpos + 3*g); + mji_copy3(res, obj->data->geom_xpos + 3*g); } // return flex element position else if (e >= 0) { - mju_copy3(res, obj->data->flexelem_aabb + 6*(obj->model->flex_elemadr[f]+e)); + mji_copy3(res, obj->data->flexelem_aabb + 6*(obj->model->flex_elemadr[f]+e)); } // return flex vertex position else { - mju_copy3(res, obj->data->flexvert_xpos + 3*(obj->model->flex_vertadr[f]+v)); + mji_copy3(res, obj->data->flexvert_xpos + 3*(obj->model->flex_vertadr[f]+v)); } } @@ -119,7 +120,7 @@ static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) { // compute mean mju_zero3(res); for (int i=0; i < 6; i++) { - mju_addTo3(res, obj->prism[i]); + mji_addTo3(res, obj->prism[i]); } mju_scl3(res, res, 1.0/6.0); } @@ -157,9 +158,7 @@ static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNu // point support function void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjtNum* pos = obj->data->geom_xpos + 3*obj->geom; - res[0] = pos[0]; - res[1] = pos[1]; - res[2] = pos[2]; + mji_copy3(res, pos); } @@ -448,7 +447,7 @@ static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) } // copy best point - mju_copy3(res, obj->prism[ibest]); + mji_copy3(res, obj->prism[ibest]); } @@ -466,7 +465,7 @@ static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f]; // find element vertex with largest projection along dir - mju_copy3(res, vert+3*edata[0]); + mji_copy3(res, vert+3*edata[0]); mjtNum best = mju_dot3(res, dir); for (int i=1; i <= dim; i++) { mjtNum dot = mju_dot3(vert+3*edata[i], dir); @@ -474,19 +473,19 @@ static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { // better vertex found: assign if (dot > best) { best = dot; - mju_copy3(res, vert+3*edata[i]); + mji_copy3(res, vert+3*edata[i]); } } // add radius and margin/2 - mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin); + mji_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin); return; } // flex vertex else { const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert); - mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin); + mji_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin); return; } } @@ -512,7 +511,7 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f]; // find element vertex with largest projection along dir - mju_copy3(res, vert+3*edata[0]); + mji_copy3(res, vert+3*edata[0]); mjtNum best = mju_dot3(res, dir); for (int i=1; i <= dim; i++) { mjtNum dot = mju_dot3(vert+3*edata[i], dir); @@ -520,19 +519,19 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { // better vertex found: assign if (dot > best) { best = dot; - mju_copy3(res, vert+3*edata[i]); + mji_copy3(res, vert+3*edata[i]); } } // add radius and margin/2 - mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin); + mji_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin); return; } // flex vertex else { const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert); - mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin); + mji_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin); return; } } @@ -551,12 +550,12 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { // compute result according to geom type switch ((mjtGeom) obj->geom_type) { case mjGEOM_SPHERE: - mju_scl3(res, local_dir, size[0]); + mji_scl3(res, local_dir, size[0]); break; case mjGEOM_CAPSULE: // start with sphere - mju_scl3(res, local_dir, size[0]); + mji_scl3(res, local_dir, size[0]); // add cylinder contribution res[2] += mju_sign(local_dir[2]) * size[1]; @@ -695,7 +694,7 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mju_mulMatVec3(res, d->geom_xmat+9*g, res); // add geom position - mju_addTo3(res, d->geom_xpos+3*g); + mji_addTo3(res, d->geom_xpos+3*g); } @@ -839,8 +838,8 @@ static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, m // fill in contact data con->dist = margin-depth; - mju_copy3(con->frame, dir.v); - mju_copy3(con->pos, pos.v); + mji_copy3(con->frame, dir.v); + mji_copy3(con->pos, pos.v); mju_zero3(con->frame+3); // both geoms: fix contact frame normal @@ -875,14 +874,14 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], mju_copy(xmat, mat, 9); // vector to rotation origin: rel = origin - xpos - mju_sub3(rel, origin, xpos); + mji_sub3(rel, origin, xpos); // displacement of origin due to rotation: vec = rot*rel - rel mju_mulMatVec3(vec, rot, rel); mju_subFrom3(vec, rel); // correct xpos by subtracting displacement: xpos = xpos - vec - mju_subFrom3(xpos, vec); + mji_subFrom3(xpos, vec); } @@ -942,14 +941,14 @@ int mjc_Convex(const mjModel* m, const mjData* d, // save positions and orientations of g1 and g2 mjtNum xpos1[3], xmat1[9], xpos2[3], xmat2[9]; - mju_copy3(xpos1, d->geom_xpos+3*g1); - mju_copy(xmat1, d->geom_xmat+9*g1, 9); - mju_copy3(xpos2, d->geom_xpos+3*g2); - mju_copy(xmat2, d->geom_xmat+9*g2, 9); + mji_copy3(xpos1, d->geom_xpos+3*g1); + mji_copy9(xmat1, d->geom_xmat+9*g1); + mji_copy3(xpos2, d->geom_xpos+3*g2); + mji_copy9(xmat2, d->geom_xmat+9*g2); // complete frame of initial contact mjtNum frame[9]; - mju_copy(frame, con[0].frame, 9); + mji_copy9(frame, con[0].frame); mju_makeFrame(frame); // tolerance for determining if newly found contacts are distinct @@ -967,7 +966,7 @@ int mjc_Convex(const mjModel* m, const mjData* d, // make rotation matrix rot mjtNum quat[4], rot[9]; - mju_axisAngle2Quat(quat, axis, angle); + mji_axisAngle2Quat(quat, axis, angle); mju_quat2Mat(rot, quat); // rotate g1 around initial contact point @@ -990,10 +989,10 @@ int mjc_Convex(const mjModel* m, const mjData* d, } // reset positions and orientations of g1 and g2 - mju_copy3(d->geom_xpos+3*g1, xpos1); - mju_copy(d->geom_xmat+9*g1, xmat1, 9); - mju_copy3(d->geom_xpos+3*g2, xpos2); - mju_copy(d->geom_xmat+9*g2, xmat2, 9); + mji_copy3(d->geom_xpos+3*g1, xpos1); + mji_copy9(d->geom_xmat+9*g1, xmat1); + mji_copy3(d->geom_xpos+3*g2, xpos2); + mji_copy9(d->geom_xmat+9*g2, xmat2); } } } @@ -1022,17 +1021,17 @@ static int addplanemesh(mjContact* con, const float vertex[3], // pnt-pos difference vector mjtNum dif[3]; - mju_sub3(dif, pnt, pos1); + mji_sub3(dif, pnt, pos1); // set distance con->dist = mju_dot3(normal1, dif); // set position - mju_copy3(con->pos, pnt); - mju_addToScl3(con->pos, normal1, -0.5*con->dist); + mji_copy3(con->pos, pnt); + mji_addToScl3(con->pos, normal1, -0.5*con->dist); // set frame - mju_copy3(con->frame, normal1); + mji_copy3(con->frame, normal1); mju_zero3(con->frame+3); return 1; @@ -1052,7 +1051,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, mjccd_support(&obj, &dir, &vec); // compute normal distance, return if too far - mju_sub3(dif, vec.v, pos1); + mji_sub3(dif, vec.v, pos1); dist = mju_dot3(normal, dif); if (dist > margin) { return 0; @@ -1060,9 +1059,9 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, // fill in contact data con->dist = dist; - mju_copy3(con->pos, vec.v); - mju_addToScl3(con->pos, normal, -0.5*dist); - mju_copy3(con->frame, normal); + mji_copy3(con->pos, vec.v); + mji_addToScl3(con->pos, normal, -0.5*dist); + mji_copy3(con->frame, normal); mju_zero3(con->frame+3); //--------------- add all/connected vertices below margin @@ -1085,7 +1084,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, mju_mulMatTVec3(locdir, d->geom_xmat+9*g, dir.v); // inclusion threshold along locdir, relative to geom2 center - mju_sub3(dif, pos2, pos1); + mji_sub3(dif, pos2, pos1); mjtNum threshold = mju_dot3(normal, dif) - margin; // no graph data: exhaustive search @@ -1150,10 +1149,10 @@ static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) { // add vertex to prism static inline void addVert(mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) { // move old data - mju_copy3(obj->prism[0], obj->prism[1]); - mju_copy3(obj->prism[1], obj->prism[2]); - mju_copy3(obj->prism[3], obj->prism[4]); - mju_copy3(obj->prism[4], obj->prism[5]); + mji_copy3(obj->prism[0], obj->prism[1]); + mji_copy3(obj->prism[1], obj->prism[2]); + mji_copy3(obj->prism[3], obj->prism[4]); + mji_copy3(obj->prism[4], obj->prism[5]); // add new vertex at last position obj->prism[2][0] = obj->prism[5][0] = x; @@ -1165,10 +1164,10 @@ static inline void addVert(mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) { // add vertex to prism static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, mjtNum dy, mjtNum margin) { // move old data - mju_copy3(obj->prism[0], obj->prism[1]); - mju_copy3(obj->prism[1], obj->prism[2]); - mju_copy3(obj->prism[3], obj->prism[4]); - mju_copy3(obj->prism[4], obj->prism[5]); + mji_copy3(obj->prism[0], obj->prism[1]); + mji_copy3(obj->prism[1], obj->prism[2]); + mji_copy3(obj->prism[3], obj->prism[4]); + mji_copy3(obj->prism[4], obj->prism[5]); int dr = 1 - i; @@ -1226,14 +1225,14 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // express geom2 mat in hfield frame mjtNum mat[9]; - mju_mulMatTMat3(mat, mat1, mat2); + mji_mulMatTMat3(mat, mat1, mat2); // save mat2 and pos2, replace with relative frame mjtNum savemat2[9], savepos2[3]; - mju_copy9(savemat2, mat2); - mju_copy3(savepos2, pos2); - mju_copy9(mat2, mat); - mju_copy3(pos2, pos); + mji_copy9(savemat2, mat2); + mji_copy3(savepos2, pos2); + mji_copy9(mat2, mat); + mji_copy3(pos2, pos); mjtNum dir[3] = {0, 0, 0}, res[3]; @@ -1275,8 +1274,8 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, if ((xmin - margin > size0) || (xmax + margin < -size0) || (ymin - margin > size1) || (ymax + margin < -size1) || (zmin - margin > size2) || (zmax + margin < -size3)) { - mju_copy9(mat2, savemat2); - mju_copy3(pos2, savepos2); + mji_copy9(mat2, savemat2); + mji_copy3(pos2, savepos2); return 0; } @@ -1332,9 +1331,9 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, && !ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[ncon].dist = -depth; - mju_mulMatVec3(con[ncon].frame, mat1, dirccd.v); - mju_mulMatVec3(con[ncon].pos, mat1, vecccd.v); - mju_addTo3(con[ncon].pos, pos1); + mji_mulMatVec3(con[ncon].frame, mat1, dirccd.v); + mji_mulMatVec3(con[ncon].pos, mat1, vecccd.v); + mji_addTo3(con[ncon].pos, pos1); mju_zero3(con[ncon].frame+3); // force out of all loops if max contacts reached @@ -1350,8 +1349,8 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, } // restore mat2 and pos2 - mju_copy9(mat2, savemat2); - mju_copy3(pos2, savepos2); + mji_copy9(mat2, savemat2); + mji_copy3(pos2, savepos2); if (mjDISABLED(mjDSBL_NATIVECCD)) { // fix contact normals @@ -1401,7 +1400,7 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum // new point on ellipsoid mjtNum pnt[3]; - mju_addScl3(pnt, pos, nrm, x); + mji_addScl3(pnt, pos, nrm, x); // normal at new point mjtNum newnrm[3] = {pnt[0]*S2inv[0], pnt[1]*S2inv[1], pnt[2]*S2inv[2]}; @@ -1409,7 +1408,7 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum // save change and assign mjtNum change = mju_dist3(nrm, newnrm); - mju_copy3(nrm, newnrm); + mji_copy3(nrm, newnrm); // terminate if converged if (change < tolerance) { @@ -1470,7 +1469,7 @@ static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum } -// entry point +// fix normals if required void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2) { mjtNum dst1, dst2; @@ -1517,7 +1516,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in // process according to type switch (type[i]) { case mjGEOM_SPHERE: - mju_copy3(nrm, pos); + mji_copy3(nrm, pos); processed[i] = 1; break; @@ -1591,25 +1590,25 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in // normalize and map normal to global frame if (processed[i]) { mju_normalize3(nrm); - mju_mulMatVec3(normal[i], mat, nrm); + mji_mulMatVec3(normal[i], mat, nrm); } } } // both processed: average if (processed[0] && processed[1]) { - mju_sub3(con->frame, normal[0], normal[1]); + mji_sub3(con->frame, normal[0], normal[1]); mju_normalize3(con->frame); } // first processed: copy else if (processed[0]) { - mju_copy3(con->frame, normal[0]); + mji_copy3(con->frame, normal[0]); } // second processed: copy reverse else if (processed[1]) { - mju_scl3(con->frame, normal[1], -1); + mji_scl3(con->frame, normal[1], -1); } // clear second frame axis if processed, just in case @@ -1676,16 +1675,16 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // save elem vertices, transform to hfield frame mjtNum savevert[4][3]; for (int i=0; i <= dim; i++) { - mju_copy3(savevert[i], evert[i]); - mju_sub3(vec, evert[i], hpos); - mju_mulMatTVec(evert[i], hmat, vec, 3, 3); + mji_copy3(savevert[i], evert[i]); + mji_sub3(vec, evert[i], hpos); + mji_mulMatTVec3(evert[i], hmat, vec); } // save elem center, transform to hfield frame mjtNum savecenter[3]; - mju_copy3(savecenter, ecenter); - mju_sub3(vec, ecenter, hpos); - mju_mulMatTVec(ecenter, hmat, vec, 3, 3); + mji_copy3(savecenter, ecenter); + mji_sub3(vec, ecenter, hpos); + mji_mulMatTVec3(ecenter, hmat, vec); // compute elem bounding box (in hfield frame) xmin = xmax = evert[0][0]; @@ -1706,9 +1705,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, (zmin-margin > hsize[2]) || (zmax+margin < -hsize[3])) { // restore vertices and center for (int i=0; i <= dim; i++) { - mju_copy3(evert[i], savevert[i]); + mji_copy3(evert[i], savevert[i]); } - mju_copy3(ecenter, savecenter); + mji_copy3(ecenter, savecenter); return 0; } @@ -1768,9 +1767,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; - mju_mulMatVec3(con[cnt].frame, hmat, dirccd.v); - mju_mulMatVec3(con[cnt].pos, hmat, vecccd.v); - mju_addTo3(con[cnt].pos, hpos); + mji_mulMatVec3(con[cnt].frame, hmat, dirccd.v); + mji_mulMatVec3(con[cnt].pos, hmat, vecccd.v); + mji_addTo3(con[cnt].pos, hpos); mju_zero3(con[cnt].frame+3); // count, stop if max number reached @@ -1790,9 +1789,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // restore elem vertices and center for (int i=0; i <= dim; i++) { - mju_copy3(evert[i], savevert[i]); + mji_copy3(evert[i], savevert[i]); } - mju_copy3(ecenter, savecenter); + mji_copy3(ecenter, savecenter); return cnt; } diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index 2530df77..9f28f621 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -16,6 +16,7 @@ #include #include +#include "engine/engine_inline.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_misc.h" #include "engine/engine_util_spatial.h" @@ -41,8 +42,8 @@ static int mjraw_PlaneSphere(mjContact* con, mjtNum margin, // depth and position con[0].dist = cdist - size2[0]; - mju_scl3(tmp, con[0].frame, -con[0].dist/2 - size2[0]); - mju_add3(con[0].pos, pos2, tmp); + mji_scl3(tmp, con[0].frame, -con[0].dist / 2 - size2[0]); + mji_add3(con[0].pos, pos2, tmp); mju_zero3(con[0].frame+3); return 1; @@ -77,10 +78,10 @@ int mjc_PlaneCapsule(const mjModel* m, const mjData* d, // align contact frames with capsule axis if (n1) { - mju_copy3(con->frame+3, axis); + mji_copy3(con->frame + 3, axis); } if (n2) { - mju_copy3((con+n1)->frame+3, axis); + mji_copy3((con + n1)->frame + 3, axis); } return n1+n2; @@ -136,10 +137,10 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d, int cnt = 0; if (dist0 + prjaxis + prjvec <= margin) { con[cnt].dist = dist0 + prjaxis + prjvec; - mju_add3(con[cnt].pos, pos2, vec); - mju_addTo3(con[cnt].pos, axis); - mju_addToScl3(con[cnt].pos, normal, -con[cnt].dist*0.5); - mju_copy3(con[cnt].frame, normal); + mji_add3(con[cnt].pos, pos2, vec); + mji_addTo3(con[cnt].pos, axis); + mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); + mji_copy3(con[cnt].frame, normal); mju_zero3(con[cnt].frame+3); cnt++; } else { @@ -149,10 +150,10 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d, // check second point, construct contact if (dist0 - prjaxis + prjvec <= margin) { con[cnt].dist = dist0 - prjaxis + prjvec; - mju_add3(con[cnt].pos, pos2, vec); - mju_subFrom3(con[cnt].pos, axis); - mju_addToScl3(con[cnt].pos, normal, -con[cnt].dist*0.5); - mju_copy3(con[cnt].frame, normal); + mji_add3(con[cnt].pos, pos2, vec); + mji_subFrom3(con[cnt].pos, axis); + mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); + mji_copy3(con[cnt].frame, normal); mju_zero3(con[cnt].frame+3); cnt++; } @@ -162,27 +163,27 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d, if (dist0 + prjaxis + prjvec1 <= margin) { // compute sideways vector: vec1 mjtNum vec1[3]; - mju_cross(vec1, vec, axis); + mji_cross(vec1, vec, axis); mju_normalize3(vec1); - mju_scl3(vec1, vec1, size2[0]*mju_sqrt(3.0)/2); + mji_scl3(vec1, vec1, size2[0] * mju_sqrt(3.0) / 2); // add point A con[cnt].dist = dist0 + prjaxis + prjvec1; - mju_add3(con[cnt].pos, pos2, vec1); - mju_addTo3(con[cnt].pos, axis); - mju_addToScl3(con[cnt].pos, vec, -0.5); - mju_addToScl3(con[cnt].pos, normal, -con[cnt].dist*0.5); - mju_copy3(con[cnt].frame, normal); + mji_add3(con[cnt].pos, pos2, vec1); + mji_addTo3(con[cnt].pos, axis); + mji_addToScl3(con[cnt].pos, vec, -0.5); + mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); + mji_copy3(con[cnt].frame, normal); mju_zero3(con[cnt].frame+3); cnt++; // add point B con[cnt].dist = dist0 + prjaxis + prjvec1; - mju_sub3(con[cnt].pos, pos2, vec1); - mju_addTo3(con[cnt].pos, axis); - mju_addToScl3(con[cnt].pos, vec, -0.5); - mju_addToScl3(con[cnt].pos, normal, -con[cnt].dist*0.5); - mju_copy3(con[cnt].frame, normal); + mji_sub3(con[cnt].pos, pos2, vec1); + mji_addTo3(con[cnt].pos, axis); + mji_addToScl3(con[cnt].pos, vec, -0.5); + mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5); + mji_copy3(con[cnt].frame, normal); mju_zero3(con[cnt].frame+3); cnt++; } @@ -222,11 +223,11 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d, // construct contact con[cnt].dist = dist + ldist; - mju_copy3(con[cnt].frame, norm); + mji_copy3(con[cnt].frame, norm); mju_zero3(con[cnt].frame+3); - mju_addTo3(corner, pos2); - mju_scl3(vec, norm, -con[cnt].dist/2); - mju_add3(con[cnt].pos, corner, vec); + mji_addTo3(corner, pos2); + mji_scl3(vec, norm, -con[cnt].dist / 2); + mji_add3(con[cnt].pos, corner, vec); // count; max is 4 if (++cnt >= 4) { @@ -254,7 +255,7 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin, // depth and normal con[0].dist = mju_sqrt(cdist_sqr) - size1[0] - size2[0]; - mju_sub3(con[0].frame, pos2, pos1); + mji_sub3(con[0].frame, pos2, pos1); mjtNum len = mju_normalize3(con[0].frame); // if centers are the same, norm = cross-product of z axes @@ -262,13 +263,13 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin, if (len < mjMINVAL) { mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum axis2[3] = {mat2[2], mat2[5], mat2[8]}; - mju_cross(con[0].frame, axis1, axis2); + mji_cross(con[0].frame, axis1, axis2); mju_normalize3(con[0].frame); } // position - mju_scl3(con[0].pos, con[0].frame, size1[0] + con[0].dist/2); - mju_addTo3(con[0].pos, pos1); + mji_scl3(con[0].pos, con[0].frame, size1[0] + con[0].dist / 2); + mji_addTo3(con[0].pos, pos1); mju_zero3(con[0].frame+3); return 1; @@ -296,8 +297,8 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin, mjtNum x = mju_clip(mju_dot3(axis, vec), -len, len); // find nearest point on segment, do sphere-sphere test - mju_scl3(vec, axis, x); - mju_addTo3(vec, pos2); + mji_scl3(vec, axis, x); + mji_addTo3(vec, pos2); return mjraw_SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size2); } @@ -324,8 +325,8 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d, mjtNum vec[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]}; mjtNum x = mju_dot3(axis, vec); mjtNum a_proj[3], p_proj[3]; - mju_scl3(a_proj, axis, x); - mju_sub3(p_proj, vec, a_proj); + mji_scl3(a_proj, axis, x); + mji_sub3(p_proj, vec, a_proj); mjtNum p_proj_sqr = mju_dot3(p_proj, p_proj); // get collision type @@ -343,7 +344,7 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d, // side collision: use sphere-sphere if (collide_side) { - mju_addTo3(a_proj, pos2); + mji_addTo3(a_proj, pos2); return mjraw_SphereSphere(con, margin, pos1, mat1, size1, a_proj, mat2, size2); } @@ -372,10 +373,11 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d, } // otherwise corner collision: use sphere-sphere - mju_scl3(p_proj, p_proj, size2[0] / mju_sqrt(p_proj_sqr)); // denominator cannot be 0 - mju_scl3(vec, axis, x > 0 ? height : -height); - mju_addTo3(vec, p_proj); - mju_addTo3(vec, pos2); + mji_scl3(p_proj, p_proj, + size2[0] / mju_sqrt(p_proj_sqr)); // denominator cannot be 0 + mji_scl3(vec, axis, x > 0 ? height : -height); + mji_addTo3(vec, p_proj); + mji_addTo3(vec, pos2); // sphere-sphere with point sphere at the corner mjtNum size_zero[1] = {0}; @@ -423,10 +425,10 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, // find nearest points, do sphere-sphere test mjtNum vec1[3], vec2[3]; - mju_scl3(vec1, axis1, x1); - mju_addTo3(vec1, pos1); - mju_scl3(vec2, axis2, x2); - mju_addTo3(vec2, pos2); + mji_scl3(vec1, axis1, x1); + mji_addTo3(vec1, pos1); + mji_scl3(vec2, axis2, x2); + mji_addTo3(vec2, pos2); return mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2); } @@ -435,19 +437,19 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, else { // x1 = 1 mjtNum vec1[3]; - mju_add3(vec1, pos1, axis1); + mji_add3(vec1, pos1, axis1); mjtNum x2 = mju_clip((v - mb) / mc, -1, 1); mjtNum vec2[3]; - mju_scl3(vec2, axis2, x2); - mju_addTo3(vec2, pos2); + mji_scl3(vec2, axis2, x2); + mji_addTo3(vec2, pos2); int n1 = mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2); // x1 = -1 - mju_sub3(vec1, pos1, axis1); + mji_sub3(vec1, pos1, axis1); x2 = mju_clip((v + mb) / mc, -1, 1); - mju_scl3(vec2, axis2, x2); - mju_addTo3(vec2, pos2); + mji_scl3(vec2, axis2, x2); + mji_addTo3(vec2, pos2); int n2 = mjraw_SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2); // return if two contacts already found @@ -456,10 +458,10 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, } // x2 = 1 - mju_add3(vec2, pos2, axis2); + mji_add3(vec2, pos2, axis2); mjtNum x1 = mju_clip((u - mb) / ma, -1, 1); - mju_scl3(vec1, axis1, x1); - mju_addTo3(vec1, pos1); + mji_scl3(vec1, axis1, x1); + mji_addTo3(vec1, pos1); int n3 = mjraw_SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2); // return if two contacts already found @@ -468,10 +470,10 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, } // x2 = -1 - mju_sub3(vec2, pos2, axis2); + mji_sub3(vec2, pos2, axis2); x1 = mju_clip((u + mb) / ma, -1, 1); - mju_scl3(vec1, axis1, x1); - mju_addTo3(vec1, pos1); + mji_scl3(vec1, axis1, x1); + mji_addTo3(vec1, pos1); int n4 = mjraw_SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2); return n1+n2+n3+n4; @@ -533,7 +535,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, // N is normal to triangle plane mjtNum N[3]; - mju_cross(N, A, B); + mji_cross(N, A, B); mju_normalize3(N); // dstS is signed distance from S to plane; exit if too large @@ -544,22 +546,22 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, // P is projection of S in triangle plane mjtNum P[3]; - mju_addScl3(P, S, N, -dstS); + mji_addScl3(P, S, N, -dstS); // construct orthogonal axes (V1~A, V2) of triangle plane mjtNum V1[3], V2[3]; - mju_copy3(V1, A); + mji_copy3(V1, A); mjtNum lenA = mju_normalize3(V1); - mju_cross(V2, N, A); + mji_cross(V2, N, A); mju_normalize3(V2); // triangle is (o,a,b), sphere center is p mjtNum o[2] = {0, 0}; - mjtNum a[2] = {lenA, 0}; // equals {mju_dot3(V1, A), mju_dot3(V2, A)} + mjtNum a[2] = {lenA, 0}; // equals {mju_dot3(V1, A), mju_dot3(V2, A)} mjtNum b[2] = {mju_dot3(V1, B), mju_dot3(V2, B)}; mjtNum p[2] = {mju_dot3(V1, P), mju_dot3(V2, P)}; - // copmuted signs of areas of (p,o,a), (p,a,b), (p,b,o) + // computed signs of areas of (p,o,a), (p,a,b), (p,b,o) mjtNum sign1 = areaSign(p, o, a); mjtNum sign2 = areaSign(p, a, b); mjtNum sign3 = areaSign(p, b, o); @@ -567,7 +569,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, // p is inside triangle if (sign1 == sign2 && sign2 == sign3) { // P is nearest point to S within triangle - mju_copy3(X, P); + mji_copy3(X, P); } // p is not inside triangle @@ -582,8 +584,8 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, int best = (dstx[0] < dstx[1] && dstx[0] < dstx[2]) ? 0 : (dstx[1] < dstx[2] ? 1 : 2); // convert x[best] to 3D - mju_scl3(X, V1, x[best][0]); - mju_addToScl3(X, V2, x[best][1]); + mji_scl3(X, V1, x[best][0]); + mji_addToScl3(X, V2, x[best][1]); } // X is now the nearest point to S within the 3D triangle (O,A,B) @@ -598,8 +600,8 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, // construct contact con[0].dist = dst - rs - rt; - mju_addScl3(con[0].pos, s, nrm, rs + con[0].dist/2); - mju_copy3(con[0].frame, nrm); + mji_addScl3(con[0].pos, s, nrm, rs + con[0].dist / 2); + mji_copy3(con[0].frame, nrm); mju_zero3(con[0].frame+3); return 1; diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index fffce442..8c3c29f9 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -23,6 +23,7 @@ #include "engine/engine_core_constraint.h" #include "engine/engine_core_util.h" #include "engine/engine_crossplatform.h" +#include "engine/engine_inline.h" #include "engine/engine_macro.h" #include "engine/engine_memory.h" #include "engine/engine_sleep.h" @@ -67,13 +68,13 @@ void mj_kinematics1(const mjModel* m, mjData* d) { int qadr = m->jnt_qposadr[jntadr]; // copy pos and quat from qpos - mju_copy3(xpos, d->qpos+qadr); - mju_copy4(xquat, d->qpos+qadr+3); + mji_copy3(xpos, d->qpos+qadr); + mji_copy4(xquat, d->qpos+qadr+3); mju_normalize4(xquat); // assign xanchor and xaxis - mju_copy3(d->xanchor+3*jntadr, xpos); - mju_copy3(d->xaxis+3*jntadr, m->jnt_axis+3*jntadr); + mji_copy3(d->xanchor+3*jntadr, xpos); + mji_copy3(d->xaxis+3*jntadr, m->jnt_axis+3*jntadr); } // regular or no joint @@ -84,7 +85,7 @@ void mj_kinematics1(const mjModel* m, mjData* d) { mjtNum *bodypos, *bodyquat, quat[4]; if (m->body_mocapid[i] >= 0) { bodypos = d->mocap_pos + 3*m->body_mocapid[i]; - mju_copy4(quat, d->mocap_quat + 4*m->body_mocapid[i]); + mji_copy4(quat, d->mocap_quat + 4*m->body_mocapid[i]); mju_normalize4(quat); bodyquat = quat; } else { @@ -94,13 +95,13 @@ void mj_kinematics1(const mjModel* m, mjData* d) { // apply fixed translation and rotation relative to parent if (pid) { - mju_mulMatVec3(xpos, d->xmat+9*pid, bodypos); - mju_addTo3(xpos, d->xpos+3*pid); - mju_mulQuat(xquat, d->xquat+4*pid, bodyquat); + mji_mulMatVec3(xpos, d->xmat+9*pid, bodypos); + mji_addTo3(xpos, d->xpos+3*pid); + mji_mulQuat(xquat, d->xquat+4*pid, bodyquat); } else { // parent is the world - mju_copy3(xpos, bodypos); - mju_copy4(xquat, bodyquat); + mji_copy3(xpos, bodypos); + mji_copy4(xquat, bodyquat); } // accumulate joints, compute xpos and xquat for this body @@ -112,16 +113,16 @@ void mj_kinematics1(const mjModel* m, mjData* d) { mjtJoint jtype = m->jnt_type[jid]; // compute axis in global frame; ball jnt_axis is (0,0,1), set by compiler - mju_rotVecQuat(xaxis, m->jnt_axis+3*jid, xquat); + mji_rotVecQuat(xaxis, m->jnt_axis+3*jid, xquat); // compute anchor in global frame - mju_rotVecQuat(xanchor, m->jnt_pos+3*jid, xquat); - mju_addTo3(xanchor, xpos); + mji_rotVecQuat(xanchor, m->jnt_pos+3*jid, xquat); + mji_addTo3(xanchor, xpos); // apply joint transformation switch (jtype) { case mjJNT_SLIDE: - mju_addToScl3(xpos, xaxis, d->qpos[qadr] - m->qpos0[qadr]); + mji_addToScl3(xpos, xaxis, d->qpos[qadr] - m->qpos0[qadr]); break; case mjJNT_BALL: @@ -130,10 +131,10 @@ void mj_kinematics1(const mjModel* m, mjData* d) { // compute local quaternion rotation mjtNum qloc[4]; if (jtype == mjJNT_BALL) { - mju_copy4(qloc, d->qpos+qadr); + mji_copy4(qloc, d->qpos+qadr); mju_normalize4(qloc); } else { - mju_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]); + mji_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]); } // apply rotation @@ -141,8 +142,8 @@ void mj_kinematics1(const mjModel* m, mjData* d) { // correct for off-center rotation mjtNum vec[3]; - mju_rotVecQuat(vec, m->jnt_pos+3*jid, xquat); - mju_sub3(xpos, xanchor, vec); + mji_rotVecQuat(vec, m->jnt_pos+3*jid, xquat); + mji_sub3(xpos, xanchor, vec); } break; @@ -151,8 +152,8 @@ void mj_kinematics1(const mjModel* m, mjData* d) { } // assign xanchor and xaxis - mju_copy3(d->xanchor+3*jid, xanchor); - mju_copy3(d->xaxis+3*jid, xaxis); + mji_copy3(d->xanchor+3*jid, xanchor); + mji_copy3(d->xaxis+3*jid, xaxis); } } @@ -179,8 +180,8 @@ void mj_kinematics1(const mjModel* m, mjData* d) { } // assign xquat and xpos, construct xmat - mju_copy4(d->xquat+4*i, xquat); - mju_copy3(d->xpos+3*i, xpos); + mji_copy4(d->xquat+4*i, xquat); + mji_copy3(d->xpos+3*i, xpos); mju_quat2Mat(d->xmat+9*i, xquat); } } @@ -251,7 +252,7 @@ void mj_comPos(const mjModel* m, mjData* d) { for (int b=0; b < nbody; b++) { int i = sleep_filter ? d->body_awake_ind[b] : b; - mju_scl3(d->subtree_com+3*i, d->xipos+3*i, m->body_mass[i]); + mji_scl3(d->subtree_com+3*i, d->xipos+3*i, m->body_mass[i]); } // subtree_com: accumulate to parent in backward pass @@ -263,10 +264,10 @@ void mj_comPos(const mjModel* m, mjData* d) { int parent = m->body_parentid[i]; if (sleep_filter && d->body_awake[i] == mjS_ASLEEP) { mjtNum child_moment[3]; - mju_scl3(child_moment, d->subtree_com+3*i, m->body_subtreemass[i]); - mju_addTo3(d->subtree_com+3*parent, child_moment); + mji_scl3(child_moment, d->subtree_com+3*i, m->body_subtreemass[i]); + mji_addTo3(d->subtree_com+3*parent, child_moment); } else { - mju_addTo3(d->subtree_com+3*parent, d->subtree_com+3*i); + mji_addTo3(d->subtree_com+3*parent, d->subtree_com+3*i); } } @@ -275,9 +276,10 @@ void mj_comPos(const mjModel* m, mjData* d) { int i = sleep_filter ? d->body_awake_ind[b] : b; if (m->body_subtreemass[i] < mjMINVAL) { - mju_copy3(d->subtree_com+3*i, d->xipos+3*i); + mji_copy3(d->subtree_com+3*i, d->xipos+3*i); } else { - mju_scl3(d->subtree_com+3*i, d->subtree_com+3*i, 1.0/m->body_subtreemass[i]); + mju_scl3(d->subtree_com + 3 * i, d->subtree_com + 3 * i, + 1.0 / m->body_subtreemass[i]); } } @@ -289,7 +291,7 @@ void mj_comPos(const mjModel* m, mjData* d) { int i = sleep_filter ? d->body_awake_ind[b] : b; mjtNum offset[3]; - mju_sub3(offset, d->xipos+3*i, d->subtree_com+3*m->body_rootid[i]); + mji_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]); } @@ -308,7 +310,7 @@ void mj_comPos(const mjModel* m, mjData* d) { // compute com-anchor vector mjtNum offset[3], axis[3]; - mju_sub3(offset, d->subtree_com+3*m->body_rootid[i], d->xanchor+3*j); + mji_sub3(offset, d->subtree_com+3*m->body_rootid[i], d->xanchor+3*j); // create motion dof int skip = 0; @@ -377,16 +379,16 @@ void mj_camlight(const mjModel* m, mjData* d) { case mjCAMLIGHT_TRACK: case mjCAMLIGHT_TRACKCOM: // fixed global orientation - mju_copy9(d->cam_xmat+9*i, m->cam_mat0+9*i); + mji_copy9(d->cam_xmat+9*i, m->cam_mat0+9*i); // position: track camera body if (m->cam_mode[i] == mjCAMLIGHT_TRACK) { - mju_add3(d->cam_xpos+3*i, d->xpos+3*id, m->cam_pos0+3*i); + mji_add3(d->cam_xpos+3*i, d->xpos+3*id, m->cam_pos0+3*i); } // position: track subtree com else { - mju_add3(d->cam_xpos+3*i, d->subtree_com+3*id, m->cam_poscom0+3*i); + mji_add3(d->cam_xpos+3*i, d->subtree_com+3*id, m->cam_poscom0+3*i); } break; @@ -397,25 +399,25 @@ void mj_camlight(const mjModel* m, mjData* d) { mjtNum pos[3]; // get position to look at if (m->cam_mode[i] == mjCAMLIGHT_TARGETBODY) { - mju_copy3(pos, d->xpos+3*id1); + mji_copy3(pos, d->xpos+3*id1); } else { - mju_copy3(pos, d->subtree_com+3*id1); + mji_copy3(pos, d->subtree_com+3*id1); } // zaxis = -desired camera direction, in global frame mjtNum matT[9]; - mju_sub3(matT+6, d->cam_xpos+3*i, pos); + mji_sub3(matT+6, d->cam_xpos+3*i, pos); mju_normalize3(matT+6); // xaxis: orthogonal to zaxis and to (0,0,1) matT[3] = 0; matT[4] = 0; matT[5] = 1; - mju_cross(matT, matT+3, matT+6); + mji_cross(matT, matT+3, matT+6); mju_normalize3(matT); // yaxis: orthogonal to xaxis and zaxis - mju_cross(matT+3, matT+6, matT); + mji_cross(matT+3, matT+6, matT); mju_normalize3(matT+3); // set camera frame @@ -439,7 +441,7 @@ void mj_camlight(const mjModel* m, mjData* d) { // default processing for fixed mode mj_local2Global(d, d->light_xpos+3*i, 0, m->light_pos+3*i, 0, id, 0); - mju_rotVecQuat(d->light_xdir+3*i, m->light_dir+3*i, d->xquat+4*id); + mji_rotVecQuat(d->light_xdir+3*i, m->light_dir+3*i, d->xquat+4*id); // adjust for mode switch ((mjtCamLight) m->light_mode[i]) { @@ -448,16 +450,16 @@ void mj_camlight(const mjModel* m, mjData* d) { case mjCAMLIGHT_TRACK: case mjCAMLIGHT_TRACKCOM: // fixed global orientation - mju_copy3(d->light_xdir+3*i, m->light_dir0+3*i); + mji_copy3(d->light_xdir+3*i, m->light_dir0+3*i); // position: track light body if (m->light_mode[i] == mjCAMLIGHT_TRACK) { - mju_add3(d->light_xpos+3*i, d->xpos+3*id, m->light_pos0+3*i); + mji_add3(d->light_xpos+3*i, d->xpos+3*id, m->light_pos0+3*i); } // position: track subtree com else { - mju_add3(d->light_xpos+3*i, d->subtree_com+3*id, m->light_poscom0+3*i); + mji_add3(d->light_xpos+3*i, d->subtree_com+3*id, m->light_poscom0+3*i); } break; @@ -468,13 +470,13 @@ void mj_camlight(const mjModel* m, mjData* d) { // get position to look at mjtNum lookat[3]; if (m->light_mode[i] == mjCAMLIGHT_TARGETBODY) { - mju_copy3(lookat, d->xpos+3*id1); + mji_copy3(lookat, d->xpos+3*id1); } else { - mju_copy3(lookat, d->subtree_com+3*id1); + mji_copy3(lookat, d->subtree_com+3*id1); } // set dir - mju_sub3(d->light_xdir+3*i, lookat, d->light_xpos+3*i); + mji_sub3(d->light_xdir+3*i, lookat, d->light_xpos+3*i); } } @@ -554,15 +556,15 @@ void mj_flex(const mjModel* m, mjData* d) { // centered: copy body position if (m->flex_centered[f]) { for (int i=vstart; i < vend; i++) { - mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); + mji_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } } // non-centered: map from local to global else { for (int i=vstart; i < vend; i++) { - mju_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i); - mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); + mji_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i); + mji_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } } } @@ -572,13 +574,13 @@ void mj_flex(const mjModel* m, mjData* d) { mjtNum nodexpos[3*mjMAXFLEXNODES]; if (m->flex_centered[f]) { for (int i=nstart; i < nend; i++) { - mju_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]); + mji_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]); } } else { for (int i=nstart; i < nend; i++) { int j = i - nstart; - mju_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i); - mju_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]); + mji_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i); + mji_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]); } } @@ -606,8 +608,8 @@ void mj_flex(const mjModel* m, mjData* d) { // compute min and max along each global axis mjtNum xmin[3], xmax[3]; - mju_copy3(xmin, vert+3*edata[0]); - mju_copy3(xmax, vert+3*edata[0]); + mji_copy3(xmin, vert+3*edata[0]); + mji_copy3(xmax, vert+3*edata[0]); for (int i=1; i <= dim; i++) { for (int j=0; j < 3; j++) { mjtNum value = vert[3*edata[i]+j]; @@ -637,8 +639,8 @@ void mj_flex(const mjModel* m, mjData* d) { // copy element aabbs to bhv leaf aabbs for (int i=flex_bvhadr; i < flex_bvhadr+flex_bvhnum; i++) { if (m->bvh_nodeid[i] >= 0) { - mju_copy(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic), - d->flexelem_aabb + 6*(m->flex_elemadr[f] + m->bvh_nodeid[i]), 6); + mji_copy6(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic), + d->flexelem_aabb + 6*(m->flex_elemadr[f] + m->bvh_nodeid[i])); } } @@ -689,7 +691,7 @@ void mj_flex(const mjModel* m, mjData* d) { // vec = unit vector from v1 to v2, compute edge length mjtNum vec[3]; - mju_sub3(vec, pos2, pos1); + mji_sub3(vec, pos2, pos1); d->flexedge_length[ebase+e] = mju_normalize3(vec); // skip Jacobian if not needed @@ -851,7 +853,7 @@ void mj_tendon(const mjModel* m, mjData* d) { mjtNum wlen = -1; int wrapid = -1; mjtNum wpnt[12]; - mju_copy3(wpnt, d->site_xpos+3*id0); + mji_copy3(wpnt, d->site_xpos+3*id0); int wbody[4]; wbody[0] = m->site_bodyid[id0]; @@ -878,11 +880,11 @@ void mj_tendon(const mjModel* m, mjData* d) { // complete sequence, accumulate lengths if (wlen < 0) { - mju_copy3(wpnt+3, d->site_xpos+3*id1); + mji_copy3(wpnt+3, d->site_xpos+3*id1); wbody[1] = m->site_bodyid[id1]; L[i] += mju_dist3(wpnt, wpnt+3) / divisor; } else { - mju_copy3(wpnt+9, d->site_xpos+3*id1); + mji_copy3(wpnt+9, d->site_xpos+3*id1); wbody[1] = wbody[2] = m->geom_bodyid[wrapid]; wbody[3] = m->site_bodyid[id1]; L[i] += (mju_dist3(wpnt, wpnt+3) + wlen + mju_dist3(wpnt+6, wpnt+9)) / divisor; @@ -893,7 +895,7 @@ void mj_tendon(const mjModel* m, mjData* d) { if (wbody[k] != wbody[k+1]) { // get 3D position difference, normalize mjtNum dif[3]; - mju_sub3(dif, wpnt+3*k+3, wpnt+3*k); + mji_sub3(dif, wpnt+3*k+3, wpnt+3*k); mju_normalize3(dif); // sparse @@ -934,7 +936,11 @@ void mj_tendon(const mjModel* m, mjData* d) { } // assign to wrap - mju_copy(d->wrap_xpos+wrapcount*3, wpnt, (wlen < 0 ? 3 : 9)); + if (wlen < 0) { + mji_copy3(d->wrap_xpos+wrapcount*3, wpnt); + } else { + mji_copy9(d->wrap_xpos+wrapcount*3, wpnt); + } d->wrap_obj[wrapcount] = -1; if (wlen >= 0) { d->wrap_obj[wrapcount+1] = d->wrap_obj[wrapcount+2] = wrapid; @@ -947,7 +953,7 @@ void mj_tendon(const mjModel* m, mjData* d) { // assign last site before pulley or tendon end if (j == tendon_num-1 || m->wrap_type[adr+j+1] == mjWRAP_PULLEY) { - mju_copy3(d->wrap_xpos+wrapcount*3, d->site_xpos+3*id1); + mji_copy3(d->wrap_xpos+wrapcount*3, d->site_xpos+3*id1); d->wrap_obj[wrapcount] = -1; d->ten_wrapnum[i]++; wrapcount++; @@ -1009,7 +1015,7 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { // init sequence; assume it starts with site mjtNum wpnt[6]; - mju_copy3(wpnt, d->site_xpos+3*id0); + mji_copy3(wpnt, d->site_xpos+3*id0); mjtNum vel[6]; mj_objectVelocity(m, d, mjOBJ_SITE, id0, vel, /*flg_local=*/0); mjtNum wvel[6] = {vel[3], vel[4], vel[5], 0, 0, 0}; @@ -1026,9 +1032,9 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { // complete sequence wbody[1] = m->site_bodyid[id1]; - mju_copy3(wpnt+3, d->site_xpos+3*id1); + mji_copy3(wpnt+3, d->site_xpos+3*id1); mj_objectVelocity(m, d, mjOBJ_SITE, id1, vel, /*flg_local=*/0); - mju_copy3(wvel+3, vel+3); + mji_copy3(wvel+3, vel+3); // accumulate moments if consecutive points are in different bodies if (wbody[0] != wbody[1]) { @@ -1144,17 +1150,17 @@ void mj_transmission(const mjModel* m, mjData* d) { else if (m->jnt_type[id] == mjJNT_BALL) { // axis: expmap representation of quaternion mjtNum axis[3], quat[4]; - mju_copy4(quat, d->qpos+m->jnt_qposadr[id]); + mji_copy4(quat, d->qpos+m->jnt_qposadr[id]); mju_normalize4(quat); - mju_quat2Vel(axis, quat, 1); + mji_quat2Vel(axis, quat, 1); // gearAxis: rotate to parent frame if necessary mjtNum gearAxis[3]; if (m->actuator_trntype[i] == mjTRN_JOINT) { - mju_copy3(gearAxis, gear); + mji_copy3(gearAxis, gear); } else { - mju_negQuat(quat, quat); - mju_rotVecQuat(gearAxis, gear, quat); + mji_negQuat(quat, quat); + mji_rotVecQuat(gearAxis, gear, quat); } // length: axis*gearAxis @@ -1170,7 +1176,7 @@ void mj_transmission(const mjModel* m, mjData* d) { rownnz[i] = 3; // moment: gearAxis - mju_copy3(moment+adr, gearAxis); + mji_copy3(moment+adr, gearAxis); } // free joint: 6D wrench gear @@ -1181,13 +1187,13 @@ void mj_transmission(const mjModel* m, mjData* d) { // gearAxis: rotate to world frame if necessary mjtNum gearAxis[3]; if (m->actuator_trntype[i] == mjTRN_JOINT) { - mju_copy3(gearAxis, gear+3); + mji_copy3(gearAxis, gear+3); } else { mjtNum quat[4]; - mju_copy4(quat, d->qpos+m->jnt_qposadr[id]+3); + mji_copy4(quat, d->qpos+m->jnt_qposadr[id]+3); mju_normalize4(quat); mju_negQuat(quat, quat); - mju_rotVecQuat(gearAxis, gear+3, quat); + mji_rotVecQuat(gearAxis, gear+3, quat); } // dof start address @@ -1200,8 +1206,8 @@ void mj_transmission(const mjModel* m, mjData* d) { rownnz[i] = 6; // moment: gear(tran), gearAxis - mju_copy3(moment+adr, gear); - mju_copy3(moment+adr+3, gearAxis); + mji_copy3(moment+adr, gear); + mji_copy3(moment+adr+3, gearAxis); } break; @@ -1235,12 +1241,12 @@ void mj_transmission(const mjModel* m, mjData* d) { if (ok) { mju_scl3(dldv, axis, 1-av/sdet); mju_scl3(dlda, vec, 1/sdet); // use dlda as temp - mju_addTo3(dldv, dlda); + mji_addTo3(dldv, dlda); mju_scl3(dlda, vec, 1-av/sdet); } else { - mju_copy3(dlda, vec); - mju_copy3(dldv, axis); + mji_copy3(dlda, vec); + mji_copy3(dldv, axis); } // get Jacobians of axis(jacA) and vec(jac) @@ -1317,8 +1323,8 @@ void mj_transmission(const mjModel* m, mjData* d) { if (m->actuator_trnid[2*i+1] == -1) { // wrench: gear expressed in global frame mjtNum wrench[6]; - mju_mulMatVec3(wrench, d->site_xmat+9*id, gear); // translation - mju_mulMatVec3(wrench+3, d->site_xmat+9*id, gear+3); // rotation + mji_mulMatVec3(wrench, d->site_xmat+9*id, gear); // translation + mji_mulMatVec3(wrench+3, d->site_xmat+9*id, gear+3); // rotation // moment: global Jacobian projected on wrench mju_mulMatTVec(moment+adr, jac, wrench, 3, nv); // translation @@ -1389,7 +1395,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // wrench: translational gear expressed in global frame mjtNum wrench[6]; - mju_mulMatVec3(wrench, d->site_xmat+9*refid, gear); + mji_mulMatVec3(wrench, d->site_xmat+9*refid, gear); // moment: global Jacobian projected on wrench mju_mulMatTVec(moment+adr, jac, wrench, 3, nv); @@ -1401,12 +1407,12 @@ void mj_transmission(const mjModel* m, mjData* d) { // get site and refsite quats from parent bodies (avoiding mju_mat2Quat) mjtNum quat[4]; - mju_mulQuat(quat, m->site_quat+4*id, d->xquat+4*m->site_bodyid[id]); - mju_mulQuat(refquat, m->site_quat+4*refid, d->xquat+4*m->site_bodyid[refid]); + mji_mulQuat(quat, m->site_quat+4*id, d->xquat+4*m->site_bodyid[id]); + mji_mulQuat(refquat, m->site_quat+4*refid, d->xquat+4*m->site_bodyid[refid]); // convert difference to expmap (axis-angle) mjtNum vec[3]; - mju_subQuat(vec, quat, refquat); + mji_subQuat(vec, quat, refquat); // add length: dot product with gear length[i] += mju_dot3(vec, gear+3); @@ -1428,7 +1434,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // wrench: rotational gear expressed in global frame mjtNum wrench[6]; - mju_mulMatVec3(wrench, d->site_xmat+9*refid, gear+3); + mji_mulMatVec3(wrench, d->site_xmat+9*refid, gear+3); // moment_tmp: global Jacobian projected on wrench, add to moment if (!moment_tmp) moment_tmp = mjSTACKALLOC(d, nv, mjtNum); @@ -1711,7 +1717,7 @@ void mj_crb(const mjModel* m, mjData* d) { // sparse backward pass over ancestors for (int j=i; j >= 0; j = dof_parentid[j]) { // M(i,j) += cdof_j * (crb_body_i * cdof_i) - M[Madr_ij--] += mju_dot(cdof+6*j, buf, 6); + M[Madr_ij--] += mji_dot6(cdof+6*j, buf); } } } @@ -2119,7 +2125,7 @@ void mj_comVel(const mjModel* m, mjData* d) { // cvel = cvel_parent mjtNum cvel[6]; - mju_copy(cvel, d->cvel+6*m->body_parentid[i], 6); + mji_copy6(cvel, d->cvel+6*m->body_parentid[i]); // cvel = cvel_parent + cdof * qvel, cdofdot = cvel x cdof int dofnum = m->body_dofnum[i]; @@ -2144,9 +2150,9 @@ void mj_comVel(const mjModel* m, mjData* d) { case mjJNT_BALL: // compute all 3 cdofdots using parent velocity - mju_crossMotion(cdofdot+6*(j+0), cvel, d->cdof+6*(bda+j+0)); - mju_crossMotion(cdofdot+6*(j+1), cvel, d->cdof+6*(bda+j+1)); - mju_crossMotion(cdofdot+6*(j+2), cvel, d->cdof+6*(bda+j+2)); + mji_crossMotion(cdofdot+6*(j+0), cvel, d->cdof+6*(bda+j+0)); + mji_crossMotion(cdofdot+6*(j+1), cvel, d->cdof+6*(bda+j+1)); + mji_crossMotion(cdofdot+6*(j+2), cvel, d->cdof+6*(bda+j+2)); // update velocity mju_mulDofVec(tmp, d->cdof+6*(bda+j), d->qvel+bda+j, 3); @@ -2160,7 +2166,7 @@ void mj_comVel(const mjModel* m, mjData* d) { // in principle we should use the new velocity to compute cdofdot, // but it makes no difference because crossMotion(cdof, cdof) = 0, // and using the old velocity may be more accurate numerically - mju_crossMotion(cdofdot+6*j, cvel, d->cdof+6*(bda+j)); + mji_crossMotion(cdofdot+6*j, cvel, d->cdof+6*(bda+j)); // update velocity mju_mulDofVec(tmp, d->cdof+6*(bda+j), d->qvel+bda+j, 1); @@ -2169,7 +2175,7 @@ void mj_comVel(const mjModel* m, mjData* d) { } // assign cvel, cdofdot - mju_copy(d->cvel+6*i, cvel, 6); + mji_copy6(d->cvel+6*i, cvel); mju_copy(d->cdof_dot+6*bda, cdofdot, 6*dofnum); } } @@ -2199,7 +2205,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { dv[0] *= m->body_inertia[3*i]; dv[1] *= m->body_inertia[3*i+1]; dv[2] *= m->body_inertia[3*i+2]; - mju_mulMatVec3(d->subtree_angmom+3*i, d->ximat+9*i, dv); + mji_mulMatVec3(d->subtree_angmom+3*i, d->ximat+9*i, dv); } // subtree linear velocity @@ -2208,7 +2214,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { // non-world: add linear momentum to parent if (i) { - mju_addTo3(d->subtree_linvel+3*m->body_parentid[i], d->subtree_linvel+3*i); + mji_addTo3(d->subtree_linvel+3*m->body_parentid[i], d->subtree_linvel+3*i); } // convert linear momentum to linear velocity @@ -2227,22 +2233,22 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { mju_sub3(dx, d->xipos+3*i, d->subtree_com+3*i); mju_sub3(dv, body_vel+6*i+3, d->subtree_linvel+3*i); mju_scl3(dp, dv, m->body_mass[i]); - mju_cross(dL, dx, dp); + mji_cross(dL, dx, dp); // add to subtree i - mju_addTo3(d->subtree_angmom+3*i, dL); + mji_addTo3(d->subtree_angmom+3*i, dL); // add to parent - mju_addTo3(d->subtree_angmom+3*parent, d->subtree_angmom+3*i); + mji_addTo3(d->subtree_angmom+3*parent, d->subtree_angmom+3*i); // momentum wrt parent mju_sub3(dx, d->subtree_com+3*i, d->subtree_com+3*parent); mju_sub3(dv, d->subtree_linvel+3*i, d->subtree_linvel+3*parent); mju_scl3(dv, dv, m->body_subtreemass[i]); - mju_cross(dL, dx, dv); + mji_cross(dL, dx, dv); // add to parent - mju_addTo3(d->subtree_angmom+3*parent, dL); + mji_addTo3(d->subtree_angmom+3*parent, dL); } mj_freeStack(d); @@ -2290,7 +2296,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { mju_mulInertVec(loc_cfrc_body+6*i, d->cinert+10*i, loc_cacc+6*i); mju_mulInertVec(tmp, d->cinert+10*i, d->cvel+6*i); mjtNum tmp1[6]; - mju_crossForce(tmp1, d->cvel+6*i, tmp); + mji_crossForce(tmp1, d->cvel+6*i, tmp); mju_addTo(loc_cfrc_body+6*i, tmp1, 6); } @@ -2310,7 +2316,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { // result = cdof * cfrc_body for (int v=0; v < nv; v++) { int i = sleep_filter ? d->dof_awake_ind[v] : v; - result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6); + result[i] = mji_dot6(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i]); } mj_freeStack(d); @@ -2334,8 +2340,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { 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); - mju_copy3(cfrc+3, d->xfrc_applied+6*i); + mji_copy3(cfrc, d->xfrc_applied+6*i+3); + mji_copy3(cfrc+3, d->xfrc_applied+6*i); // map force from application point to com; both world-oriented mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[i], d->xipos+3*i, 0); @@ -2403,9 +2409,9 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { case mjEQ_CONNECT: case mjEQ_WELD: // cfrc = world-oriented torque:force vector - mju_copy3(cfrc + 3, d->efc_force + i); + mji_copy3(cfrc + 3, d->efc_force + i); if (m->eq_type[id] == mjEQ_WELD) { - mju_copy3(cfrc, d->efc_force + i + 3); + mji_copy3(cfrc, d->efc_force + i + 3); } else { mju_zero3(cfrc); // no torque from connect } @@ -2490,7 +2496,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // cfrc_body = cinert * cacc + cvel x (cinert * cvel) mju_mulInertVec(cfrc_body, d->cinert+10*j, d->cacc+6*j); mju_mulInertVec(cfrc_corr, d->cinert+10*j, d->cvel+6*j); - mju_crossForce(cfrc, d->cvel+6*j, cfrc_corr); + mji_crossForce(cfrc, d->cvel+6*j, cfrc_corr); mju_addTo(cfrc_body, cfrc, 6); // set cfrc_int = cfrc_body - cfrc_ext diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index 971b3fc7..cfe3d099 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -18,6 +18,7 @@ #include #include +#include "engine/engine_inline.h" #include "engine/engine_memory.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" @@ -214,7 +215,7 @@ void mj_jac(const mjModel* m, const mjData* d, // construct translation jacobian (correct for rotation) if (jacp) { mjtNum tmp[3]; - mju_cross(tmp, cdof, offset); + mji_cross(tmp, cdof, offset); jacp[i+0*nv] = cdof[3] + tmp[0]; jacp[i+1*nv] = cdof[4] + tmp[1]; jacp[i+2*nv] = cdof[5] + tmp[2]; @@ -358,7 +359,7 @@ void mj_jacSparse(const mjModel* m, const mjData* d, // construct translation jacobian (correct for rotation) if (jacp) { mjtNum tmp[3]; - mju_cross(tmp, cdof, offset); + mji_cross(tmp, cdof, offset); jacp[ci+0*NV] = cdof[3] + tmp[0]; jacp[ci+1*NV] = cdof[4] + tmp[1]; @@ -410,7 +411,7 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d, // construct translation jacobian (correct for rotation) if (jacdifp) { mjtNum tmp[3]; - mju_cross(tmp, cdof, offset); + mji_cross(tmp, cdof, offset); // plus sign if (flg_second) { @@ -616,7 +617,7 @@ void mj_jacDot(const mjModel* m, const mjData* d, // backward pass over dof ancestor chain while (i >= 0) { mjtNum cdof_dot[6]; - mju_copy(cdof_dot, d->cdof_dot+6*i, 6); + mji_copy6(cdof_dot, d->cdof_dot+6*i); mjtNum* cdof = d->cdof+6*i; // check for quaternion @@ -626,7 +627,7 @@ void mj_jacDot(const mjModel* m, const mjData* d, // compute cdof_dot for quaternion (use current body cvel) if (is_quat) { - mju_crossMotion(cdof_dot, d->cvel+6*m->dof_bodyid[i], cdof); + mji_crossMotion(cdof_dot, d->cvel+6*m->dof_bodyid[i], cdof); } // construct rotation jacobian @@ -640,11 +641,11 @@ void mj_jacDot(const mjModel* m, const mjData* d, if (jacp) { // first correction term, account for varying cdof mjtNum tmp1[3]; - mju_cross(tmp1, cdof_dot, offset); + mji_cross(tmp1, cdof_dot, offset); // second correction term, account for point translational velocity mjtNum tmp2[3]; - mju_cross(tmp2, cdof, pvel + 3); + mji_cross(tmp2, cdof, pvel + 3); jacp[i+0*nv] += cdof_dot[3] + tmp1[0] + tmp2[0]; jacp[i+1*nv] += cdof_dot[4] + tmp1[1] + tmp2[1]; @@ -686,7 +687,7 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { // orientation of the COM (inertial) frame of b-th body mjtNum ximat[9]; - mju_copy9(ximat, d->ximat+9*b); + mji_copy9(ximat, d->ximat+9*b); // save the inertia matrix of b-th body mjtNum inertia[9] = {0}; @@ -696,13 +697,13 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { // term1 = body angular momentum about self COM in world frame mjtNum tmp1[9], tmp2[9]; - mju_mulMatMat3(tmp1, ximat, inertia); // tmp1 = ximat * inertia + mji_mulMatMat3(tmp1, ximat, inertia); // tmp1 = ximat * inertia mju_mulMatMatT3(tmp2, tmp1, ximat); // tmp2 = ximat * inertia * ximat^T mju_mulMatMat(term1, tmp2, jacr, 3, 3, nv); // term1 = ximat * inertia * ximat^T * jacr // location of body COM w.r.t subtree COM mjtNum com[3]; - mju_sub3(com, d->xipos+3*b, subtree_com); + mji_sub3(com, d->xipos+3*b, subtree_com); // skew symmetric matrix representing body_com vector mjtNum com_mat[9] = {0}; @@ -846,8 +847,8 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d, // add Coriolis correction due to rotating frame: acc_tran += vel_rot x vel_tran mjtNum correction[3]; - mju_cross(correction, vel, vel+3); - mju_addTo3(res+3, correction); + mji_cross(correction, vel, vel+3); + mji_addTo3(res+3, correction); } @@ -863,14 +864,14 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], case mjSAMEFRAME_NONE: case mjSAMEFRAME_BODYROT: case mjSAMEFRAME_INERTIAROT: - mju_mulMatVec3(xpos, d->xmat+9*body, pos); - mju_addTo3(xpos, d->xpos+3*body); + mji_mulMatVec3(xpos, d->xmat+9*body, pos); + mji_addTo3(xpos, d->xpos+3*body); break; case mjSAMEFRAME_BODY: - mju_copy3(xpos, d->xpos+3*body); + mji_copy3(xpos, d->xpos+3*body); break; case mjSAMEFRAME_INERTIA: - mju_copy3(xpos, d->xipos+3*body); + mji_copy3(xpos, d->xipos+3*body); break; } } @@ -880,16 +881,16 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], mjtNum tmp[4]; switch (sf) { case mjSAMEFRAME_NONE: - mju_mulQuat(tmp, d->xquat+4*body, quat); + mji_mulQuat(tmp, d->xquat+4*body, quat); mju_quat2Mat(xmat, tmp); break; case mjSAMEFRAME_BODY: case mjSAMEFRAME_BODYROT: - mju_copy9(xmat, d->xmat+9*body); + mji_copy9(xmat, d->xmat+9*body); break; case mjSAMEFRAME_INERTIA: case mjSAMEFRAME_INERTIAROT: - mju_copy9(xmat, d->ximat+9*body); + mji_copy9(xmat, d->ximat+9*body); break; } } @@ -957,4 +958,3 @@ void mj_warning(mjData* d, int warning, int info) { // increase counter d->warning[warning].number++; } - diff --git a/src/engine/engine_inline.h b/src/engine/engine_inline.h new file mode 100644 index 00000000..c52abdbf --- /dev/null +++ b/src/engine/engine_inline.h @@ -0,0 +1,464 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_ENGINE_ENGINE_INLINE_H_ +#define MUJOCO_SRC_ENGINE_ENGINE_INLINE_H_ + +#include +#include + +#ifdef __cplusplus + #define restrict __restrict__ + extern "C" { +#endif + +/* ================================================================================================= + +Private, high efficiency inlined functions for internal engine use. +Inlining on its own does nothing due to LTO, so criteria for mji_ functions are: +- More efficient assembly output due to `restrict`. +- Skipped copies due to non-alias guarantee (e.g., compare mju_cross and mji_cross). + +mji_ functions: +- Should be modified as little as possible from their mju_ counterparts. +- Should be used only on the hotpath to prevent future bugs due to aliasing. + +================================================================================================= */ + + +//------------------------------ 3D vector and matrix-vector operations ---------------------------- + +// res = vec +static inline +void mji_copy3(mjtNum* restrict res, const mjtNum *vec) { + res[0] = vec[0]; + res[1] = vec[1]; + res[2] = vec[2]; +} + + +// res = vec*scl +static inline +void mji_scl3(mjtNum* restrict res, const mjtNum vec[3], mjtNum scl) { + res[0] = vec[0] * scl; + res[1] = vec[1] * scl; + res[2] = vec[2] * scl; +} + + +// res = vec1 + vec2 +static inline +void mji_add3(mjtNum* restrict res, const mjtNum vec1[3], const mjtNum vec2[3]) { + res[0] = vec1[0] + vec2[0]; + res[1] = vec1[1] + vec2[1]; + res[2] = vec1[2] + vec2[2]; +} + + +// res = vec1 - vec2 +static inline +void mji_sub3(mjtNum* restrict res, const mjtNum vec1[3], const mjtNum vec2[3]) { + res[0] = vec1[0] - vec2[0]; + res[1] = vec1[1] - vec2[1]; + res[2] = vec1[2] - vec2[2]; +} + + +// res += vec +static inline +void mji_addTo3(mjtNum* restrict res, const mjtNum vec[3]) { + res[0] += vec[0]; + res[1] += vec[1]; + res[2] += vec[2]; +} + + +// res -= vec +static inline +void mji_subFrom3(mjtNum* restrict res, const mjtNum vec[3]) { + res[0] -= vec[0]; + res[1] -= vec[1]; + res[2] -= vec[2]; +} + + +// res += vec*scl +static inline +void mji_addToScl3(mjtNum* restrict res, const mjtNum vec[3], mjtNum scl) { + res[0] += vec[0] * scl; + res[1] += vec[1] * scl; + res[2] += vec[2] * scl; +} + + +// res = vec1 + vec2*scl +static inline +void mji_addScl3(mjtNum* restrict res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl) { + res[0] = vec1[0] + scl*vec2[0]; + res[1] = vec1[1] + scl*vec2[1]; + res[2] = vec1[2] + scl*vec2[2]; +} + + +// normalize vector, return length before normalization +// (for use in this file only) +static inline +mjtNum mji__normalize3(mjtNum vec[3]) { + mjtNum norm = mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]); + + if (norm < mjMINVAL) { + vec[0] = 1; + vec[1] = 0; + vec[2] = 0; + } else { + mjtNum normInv = 1/norm; + vec[0] *= normInv; + vec[1] *= normInv; + vec[2] *= normInv; + } + + return norm; +} + + +// multiply vector by 3D rotation matrix +static inline +void mji_mulMatVec3(mjtNum* restrict res, const mjtNum mat[9], const mjtNum vec[3]) { + res[0] = mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2]; + res[1] = mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2]; + res[2] = mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2]; +} + + +// multiply vector by transposed 3D rotation matrix +static inline +void mji_mulMatTVec3(mjtNum* restrict res, const mjtNum mat[9], const mjtNum vec[3]) { + res[0] = mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2]; + res[1] = mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2]; + res[2] = mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2]; +} + + +// multiply 3x3 matrices, +static inline +void mji_mulMatMat3(mjtNum* restrict res, const mjtNum mat1[9], const mjtNum mat2[9]) { + res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[3] + mat1[2]*mat2[6]; + res[1] = mat1[0]*mat2[1] + mat1[1]*mat2[4] + mat1[2]*mat2[7]; + res[2] = mat1[0]*mat2[2] + mat1[1]*mat2[5] + mat1[2]*mat2[8]; + res[3] = mat1[3]*mat2[0] + mat1[4]*mat2[3] + mat1[5]*mat2[6]; + res[4] = mat1[3]*mat2[1] + mat1[4]*mat2[4] + mat1[5]*mat2[7]; + res[5] = mat1[3]*mat2[2] + mat1[4]*mat2[5] + mat1[5]*mat2[8]; + res[6] = mat1[6]*mat2[0] + mat1[7]*mat2[3] + mat1[8]*mat2[6]; + res[7] = mat1[6]*mat2[1] + mat1[7]*mat2[4] + mat1[8]*mat2[7]; + res[8] = mat1[6]*mat2[2] + mat1[7]*mat2[5] + mat1[8]*mat2[8]; +} + + +// multiply 3x3 matrices, first argument transposed +static inline +void mji_mulMatTMat3(mjtNum* restrict res, const mjtNum mat1[9], const mjtNum mat2[9]) { + res[0] = mat1[0]*mat2[0] + mat1[3]*mat2[3] + mat1[6]*mat2[6]; + res[1] = mat1[0]*mat2[1] + mat1[3]*mat2[4] + mat1[6]*mat2[7]; + res[2] = mat1[0]*mat2[2] + mat1[3]*mat2[5] + mat1[6]*mat2[8]; + res[3] = mat1[1]*mat2[0] + mat1[4]*mat2[3] + mat1[7]*mat2[6]; + res[4] = mat1[1]*mat2[1] + mat1[4]*mat2[4] + mat1[7]*mat2[7]; + res[5] = mat1[1]*mat2[2] + mat1[4]*mat2[5] + mat1[7]*mat2[8]; + res[6] = mat1[2]*mat2[0] + mat1[5]*mat2[3] + mat1[8]*mat2[6]; + res[7] = mat1[2]*mat2[1] + mat1[5]*mat2[4] + mat1[8]*mat2[7]; + res[8] = mat1[2]*mat2[2] + mat1[5]*mat2[5] + mat1[8]*mat2[8]; +} + + +//------------------------------ 4D vector and matrix-vector operations ---------------------------- + +// res = vec +static inline +void mji_copy4(mjtNum* restrict res, const mjtNum data[4]) { + res[0] = data[0]; + res[1] = data[1]; + res[2] = data[2]; + res[3] = data[3]; +} + + +// normalize vector, return length before normalization +// (for use in this file only) +static inline +mjtNum mji__normalize4(mjtNum vec[4]) { + mjtNum norm = mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2] + vec[3]*vec[3]); + + if (norm < mjMINVAL) { + vec[0] = 1; + vec[1] = 0; + vec[2] = 0; + vec[3] = 0; + } else if (mju_abs(norm - 1) > mjMINVAL) { + mjtNum normInv = 1/norm; + vec[0] *= normInv; + vec[1] *= normInv; + vec[2] *= normInv; + vec[3] *= normInv; + } + + return norm; +} + + +//------------------------------ quaternion operations --------------------------------------------- + +// rotate vector by quaternion +static inline +void mji_rotVecQuat(mjtNum* restrict res, const mjtNum vec[3], const mjtNum quat[4]) { + // null quat: copy vec + if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) { + mji_copy3(res, vec); + } + + // regular processing + else { + mjtNum tmp[3]; + // tmp = q_w * v + cross(q_xyz, v) + tmp[0] = quat[0]*vec[0] + quat[2]*vec[2] - quat[3]*vec[1]; + tmp[1] = quat[0]*vec[1] + quat[3]*vec[0] - quat[1]*vec[2]; + tmp[2] = quat[0]*vec[2] + quat[1]*vec[1] - quat[2]*vec[0]; + // res = v + 2 * cross(q_xyz, t) + res[0] = vec[0] + 2 * (quat[2]*tmp[2] - quat[3]*tmp[1]); + res[1] = vec[1] + 2 * (quat[3]*tmp[0] - quat[1]*tmp[2]); + res[2] = vec[2] + 2 * (quat[1]*tmp[1] - quat[2]*tmp[0]); + } +} + + +// negate quaternion +static inline +void mji_negQuat(mjtNum* restrict res, const mjtNum quat[4]) { + res[0] = quat[0]; + res[1] = -quat[1]; + res[2] = -quat[2]; + res[3] = -quat[3]; +} + + +// multiply quaternions +static inline +void mji_mulQuat(mjtNum* restrict res, const mjtNum qa[4], const mjtNum qb[4]) { + res[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3]; + res[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2]; + res[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1]; + res[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0]; +} + + +// multiply quaternion and axis +static inline +void mji_mulQuatAxis(mjtNum* restrict res, const mjtNum quat[4], const mjtNum axis[3]) { + res[0] = -quat[1]*axis[0] - quat[2]*axis[1] - quat[3]*axis[2]; + res[1] = quat[0]*axis[0] + quat[2]*axis[2] - quat[3]*axis[1]; + res[2] = quat[0]*axis[1] + quat[3]*axis[0] - quat[1]*axis[2]; + res[3] = quat[0]*axis[2] + quat[1]*axis[1] - quat[2]*axis[0]; +} + + +// convert axisAngle to quaternion +static inline +void mji_axisAngle2Quat(mjtNum* restrict res, const mjtNum axis[3], mjtNum angle) { + // zero angle: null quat + if (angle == 0) { + res[0] = 1; + res[1] = 0; + res[2] = 0; + res[3] = 0; + } + + // regular processing + else { + mjtNum s = mju_sin(angle*0.5); + res[0] = mju_cos(angle*0.5); + res[1] = axis[0]*s; + res[2] = axis[1]*s; + res[3] = axis[2]*s; + } +} + + +// convert quaternion (corresponding to orientation difference) to 3D velocity +static inline +void mji_quat2Vel(mjtNum* restrict res, const mjtNum quat[4], mjtNum dt) { + mjtNum axis[3] = {quat[1], quat[2], quat[3]}; + mjtNum sin_a_2 = mji__normalize3(axis); + mjtNum speed = 2 * mju_atan2(sin_a_2, quat[0]); + + // when axis-angle is larger than pi, rotation is in the opposite direction + if (speed > mjPI) { + speed -= 2*mjPI; + } + speed /= dt; + + mji_scl3(res, axis, speed); +} + + +// Subtract quaternions, express as 3D velocity: qb*quat(res) = qa. +static inline +void mji_subQuat(mjtNum* restrict res, const mjtNum qa[4], const mjtNum qb[4]) { + // qdif = neg(qb)*qa + mjtNum qneg[4], qdif[4]; + mji_negQuat(qneg, qb); + mji_mulQuat(qdif, qneg, qa); + + // convert to 3D velocity + mji_quat2Vel(res, qdif, 1); +} + + +// convert 3D rotation matrix to quaternion +static inline +void mji_mat2Quat(mjtNum* restrict quat, const mjtNum mat[9]) { + // q0 largest + if (mat[0]+mat[4]+mat[8]>0) { + quat[0] = 0.5 * mju_sqrt(1 + mat[0] + mat[4] + mat[8]); + quat[1] = 0.25 * (mat[7] - mat[5]) / quat[0]; + quat[2] = 0.25 * (mat[2] - mat[6]) / quat[0]; + quat[3] = 0.25 * (mat[3] - mat[1]) / quat[0]; + } + + // q1 largest + else if (mat[0]>mat[4] && mat[0]>mat[8]) { + quat[1] = 0.5 * mju_sqrt(1 + mat[0] - mat[4] - mat[8]); + quat[0] = 0.25 * (mat[7] - mat[5]) / quat[1]; + quat[2] = 0.25 * (mat[1] + mat[3]) / quat[1]; + quat[3] = 0.25 * (mat[2] + mat[6]) / quat[1]; + } + + // q2 largest + else if (mat[4]>mat[8]) { + quat[2] = 0.5 * mju_sqrt(1 - mat[0] + mat[4] - mat[8]); + quat[0] = 0.25 * (mat[2] - mat[6]) / quat[2]; + quat[1] = 0.25 * (mat[1] + mat[3]) / quat[2]; + quat[3] = 0.25 * (mat[5] + mat[7]) / quat[2]; + } + + // q3 largest + else { + quat[3] = 0.5 * mju_sqrt(1 - mat[0] - mat[4] + mat[8]); + quat[0] = 0.25 * (mat[3] - mat[1]) / quat[3]; + quat[1] = 0.25 * (mat[2] + mat[6]) / quat[3]; + quat[2] = 0.25 * (mat[5] + mat[7]) / quat[3]; + } + + mji__normalize4(quat); +} + + +// integrate quaternion given 3D angular velocity +static inline +void mji_quatIntegrate(mjtNum* restrict quat, const mjtNum vel[3], mjtNum scale) { + mjtNum angle, tmp[4], qrot[4]; + + // form local rotation quaternion, apply + mji_copy3(tmp, vel); + angle = scale * mji__normalize3(tmp); + mji_axisAngle2Quat(qrot, tmp, angle); + mji__normalize4(quat); + mji_copy4(tmp, quat); + mji_mulQuat(quat, tmp, qrot); +} + + +//------------------------------ spatial algebra --------------------------------------------------- + +// vector cross-product, 3D +static inline +void mji_cross(mjtNum* restrict res, const mjtNum a[3], const mjtNum b[3]) { + res[0] = a[1]*b[2] - a[2]*b[1]; + res[1] = a[2]*b[0] - a[0]*b[2]; + res[2] = a[0]*b[1] - a[1]*b[0]; +} + + +// cross-product for motion vector +static inline +void mji_crossMotion(mjtNum* restrict res, const mjtNum vel[6], const mjtNum v[6]) { + res[0] = -vel[2]*v[1] + vel[1]*v[2]; + res[1] = vel[2]*v[0] - vel[0]*v[2]; + res[2] = -vel[1]*v[0] + vel[0]*v[1]; + res[3] = -vel[2]*v[4] + vel[1]*v[5]; + res[4] = vel[2]*v[3] - vel[0]*v[5]; + res[5] = -vel[1]*v[3] + vel[0]*v[4]; + + res[3] += -vel[5]*v[1] + vel[4]*v[2]; + res[4] += vel[5]*v[0] - vel[3]*v[2]; + res[5] += -vel[4]*v[0] + vel[3]*v[1]; +} + + +// cross-product for force vectors +static inline +void mji_crossForce(mjtNum* restrict res, const mjtNum vel[6], const mjtNum f[6]) { + res[0] = -vel[2]*f[1] + vel[1]*f[2]; + res[1] = vel[2]*f[0] - vel[0]*f[2]; + res[2] = -vel[1]*f[0] + vel[0]*f[1]; + res[3] = -vel[2]*f[4] + vel[1]*f[5]; + res[4] = vel[2]*f[3] - vel[0]*f[5]; + res[5] = -vel[1]*f[3] + vel[0]*f[4]; + + res[0] += -vel[5]*f[4] + vel[4]*f[5]; + res[1] += vel[5]*f[3] - vel[3]*f[5]; + res[2] += -vel[4]*f[3] + vel[3]*f[4]; +} + + +// 6D vector dot-product +static inline +mjtNum mji_dot6(const mjtNum vec1[6], const mjtNum vec2[6]) { + // match order of operations to mju_dot + return ((vec1[0] * vec2[0] + vec1[2] * vec2[2]) + + (vec1[1] * vec2[1] + vec1[3] * vec2[3])) + + (vec1[4] * vec2[4] + vec1[5] * vec2[5]); +} + + +// res = vec +static inline +void mji_copy6(mjtNum* restrict res, const mjtNum *vec) { + res[0] = vec[0]; + res[1] = vec[1]; + res[2] = vec[2]; + res[3] = vec[3]; + res[4] = vec[4]; + res[5] = vec[5]; +} + + +// res = vec +static inline +void mji_copy9(mjtNum* restrict res, const mjtNum data[9]) { + res[0] = data[0]; + res[1] = data[1]; + res[2] = data[2]; + res[3] = data[3]; + res[4] = data[4]; + res[5] = data[5]; + res[6] = data[6]; + res[7] = data[7]; + res[8] = data[8]; +} + + +#ifdef __cplusplus + } // extern "C" + #undef restrict +#endif + +#endif // MUJOCO_SRC_ENGINE_ENGINE_INLINE_H_ diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 52387fbf..fdd8ef94 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -23,6 +23,7 @@ #include "engine/engine_core_constraint.h" #include "engine/engine_core_util.h" #include "engine/engine_crossplatform.h" +#include "engine/engine_inline.h" #include "engine/engine_memory.h" #include "engine/engine_plugin.h" #include "engine/engine_sleep.h" @@ -99,9 +100,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) { { // convert quaternion difference into angular "velocity" mjtNum dif[3], quat[4]; - mju_copy4(quat, d->qpos+padr); + mji_copy4(quat, d->qpos+padr); mju_normalize4(quat); - mju_subQuat(dif, quat, m->qpos_spring + padr); + mji_subQuat(dif, quat, m->qpos_spring + padr); // apply torque d->qfrc_spring[dadr+0] = -stiffness*dif[0]; @@ -161,15 +162,15 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // flap edges mjtNum ed[3][3]; - mju_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]); - mju_sub3(ed[1], xpos + 3*v[2], xpos + 3*v[0]); - mju_sub3(ed[2], xpos + 3*v[3], xpos + 3*v[0]); + mji_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]); + mji_sub3(ed[1], xpos + 3*v[2], xpos + 3*v[0]); + mji_sub3(ed[2], xpos + 3*v[3], xpos + 3*v[0]); // forces at the vertices due to curved reference mjtNum frc[4][3]; - mju_cross(frc[1], ed[1], ed[2]); - mju_cross(frc[2], ed[2], ed[0]); - mju_cross(frc[3], ed[0], ed[1]); + mji_cross(frc[1], ed[1], ed[2]); + mji_cross(frc[2], ed[2], ed[0]); + mji_cross(frc[3], ed[0], ed[1]); frc[0][0] = -(frc[1][0] + frc[2][0] + frc[3][0]); frc[0][1] = -(frc[1][1] + frc[2][1] + frc[3][1]); frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]); @@ -226,27 +227,27 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // compute positions if (m->flex_centered[f]) { for (int i=0; i < nodenum; i++) { - mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); - mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]); + mji_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); + mji_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]); } } else { mjtNum screw[6]; for (int i=0; i < nodenum; i++) { - mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart)); - mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]); + mji_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart)); + mji_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]); mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0); - mju_copy3(vel + 3*i, screw + 3); + mji_copy3(vel + 3*i, screw + 3); } } // compute center of mass for (int i = 0; i < nodenum; i++) { - mju_addToScl3(com, xpos+3*i, 1.0/nodenum); + mji_addToScl3(com, xpos+3*i, 1.0/nodenum); } // re-center positions using center of mass for (int i = 0; i < nodenum; i++) { - mju_addToScl3(xpos+3*i, com, -1); + mji_addToScl3(xpos+3*i, com, -1); } // compute the Jacobian at the center of mass @@ -262,13 +263,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // rotate vertices to quat and add reference center of mass for (int i = 0; i < nodenum; i++) { mju_rotVecQuat(xpos+3*i, xpos+3*i, quat); - mju_addTo3(xpos+3*i, p); + mji_addTo3(xpos+3*i, p); mju_rotVecQuat(vel+3*i, vel+3*i, quat); } // compute displacement for (int i = 0; i < nodenum; i++) { - mju_addScl3(displ+3*i, xpos+3*i, xpos0+3*i, -1); + mji_addScl3(displ+3*i, xpos+3*i, xpos0+3*i, -1); } // compute force in the stretch frame @@ -281,12 +282,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mju_negQuat(quat, quat); for (int i = 0; i < nodenum; i++) { mjtNum qfrc[3], qdmp[3]; - mju_rotVecQuat(qfrc, frc+3*i, quat); - mju_rotVecQuat(qdmp, dmp+3*i, quat); + mji_rotVecQuat(qfrc, frc+3*i, quat); + mji_rotVecQuat(qdmp, dmp+3*i, quat); mju_scl3(qdmp, qdmp, m->flex_damping[f]); if (m->flex_centered[f]) { - if (has_spring) mju_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc); - if (has_damping) mju_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp); + if (has_spring) mji_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc); + if (has_damping) mji_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp); } else { if (has_spring) mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring); if (has_damping) mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper); @@ -491,7 +492,7 @@ static int mj_gravcomp(const mjModel* m, mjData* d) { int i = sleep_filter ? d->body_awake_ind[b] : b; if (m->body_gravcomp[i]) { has_gravcomp = 1; - mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i])); + mji_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i])); mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_gravcomp); } } @@ -749,12 +750,12 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) { // compute wind in local coordinates mju_zero(wind, 6); - mju_copy3(wind+3, m->opt.wind); + mji_copy3(wind+3, m->opt.wind); mju_transformSpatial(lwind, wind, 0, d->xipos+3*i, d->subtree_com+3*m->body_rootid[i], d->ximat+9*i); // subtract translational component from body velocity - mju_subFrom3(lvel+3, lwind+3); + mji_subFrom3(lvel+3, lwind+3); mju_zero(lfrc, 6); // set viscous force and torque @@ -763,10 +764,10 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) { diam = (box[0] + box[1] + box[2])/3.0; // angular viscosity - mju_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity); + mji_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity); // linear viscosity - mju_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity); + mji_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity); } // add lift and drag force and torque @@ -785,8 +786,8 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) { mju_abs(lvel[2])*lvel[2]/64.0; } // rotate to global orientation: lfrc -> bfrc - mju_mulMatVec3(bfrc, d->ximat+9*i, lfrc); - mju_mulMatVec3(bfrc+3, d->ximat+9*i, lfrc+3); + mji_mulMatVec3(bfrc, d->ximat+9*i, lfrc); + mji_mulMatVec3(bfrc+3, d->ximat+9*i, lfrc+3); // apply force and torque to body com mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_fluid); @@ -821,14 +822,14 @@ void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) { // compute wind in local coordinates mju_zero(wind, 6); - mju_copy3(wind+3, m->opt.wind); + mji_copy3(wind+3, m->opt.wind); mju_transformSpatial(lwind, wind, 0, d->geom_xpos + 3*geomid, // Frame of ref's origin. d->subtree_com + 3*m->body_rootid[bodyid], d->geom_xmat + 9*geomid); // Frame of ref's orientation. // subtract translational component from grom velocity - mju_subFrom3(lvel+3, lwind+3); + mji_subFrom3(lvel+3, lwind+3); // initialize viscous force and torque mju_zero(lfrc, 6); @@ -844,8 +845,8 @@ void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) { mju_scl(lfrc, lfrc, geom_interaction_coef, 6); // rotate to global orientation: lfrc -> bfrc - mju_mulMatVec3(bfrc, d->geom_xmat + 9*geomid, lfrc); - mju_mulMatVec3(bfrc+3, d->geom_xmat + 9*geomid, lfrc+3); + mji_mulMatVec3(bfrc, d->geom_xmat + 9*geomid, lfrc); + mji_mulMatVec3(bfrc+3, d->geom_xmat + 9*geomid, lfrc+3); // apply force and torque to body com mj_applyFT(m, d, bfrc+3, bfrc, @@ -884,13 +885,13 @@ void mj_addedMassForces(const mjtNum local_vels[6], const mjtNum local_accels[6] } mjtNum added_mass_force[3], added_mass_torque1[3], added_mass_torque2[3]; - mju_cross(added_mass_force, virtual_lin_mom, ang_vel); - mju_cross(added_mass_torque1, virtual_lin_mom, lin_vel); - mju_cross(added_mass_torque2, virtual_ang_mom, ang_vel); + mji_cross(added_mass_force, virtual_lin_mom, ang_vel); + mji_cross(added_mass_torque1, virtual_lin_mom, lin_vel); + mji_cross(added_mass_torque2, virtual_ang_mom, ang_vel); - mju_addTo3(local_force, added_mass_torque1); - mju_addTo3(local_force, added_mass_torque2); - mju_addTo3(local_force+3, added_mass_force); + mji_addTo3(local_force, added_mass_torque1); + mji_addTo3(local_force, added_mass_torque2); + mji_addTo3(local_force+3, added_mass_force); } @@ -926,7 +927,7 @@ void mj_viscousForces( const mjtNum A_max = mjPI * d_max * d_mid; mjtNum magnus_force[3]; - mju_cross(magnus_force, ang_vel, lin_vel); + mji_cross(magnus_force, ang_vel, lin_vel); magnus_force[0] *= magnus_lift_coef * fluid_density * volume; magnus_force[1] *= magnus_lift_coef * fluid_density * volume; magnus_force[2] *= magnus_lift_coef * fluid_density * volume; @@ -955,12 +956,12 @@ void mj_viscousForces( const mjtNum cos_alpha = proj_num / mju_max( mjMINVAL, mju_norm3(lin_vel) * proj_denom); mjtNum kutta_circ[3]; - mju_cross(kutta_circ, norm, lin_vel); + mji_cross(kutta_circ, norm, lin_vel); kutta_circ[0] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj; kutta_circ[1] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj; kutta_circ[2] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj; mjtNum kutta_force[3]; - mju_cross(kutta_force, kutta_circ, lin_vel); + mji_cross(kutta_force, kutta_circ, lin_vel); // viscous force and torque in Stokes flow, analytical for spherical bodies const mjtNum eq_sphere_D = 2.0/3.0 * (size[0] + size[1] + size[2]); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 78ca676e..a896dd4a 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -26,6 +26,7 @@ #include "engine/engine_core_util.h" #include "engine/engine_crossplatform.h" #include "engine/engine_memory.h" +#include "engine/engine_memory.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 0deb7915..3621ec19 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -19,6 +19,7 @@ #include #include #include // IWYU pragma: keep +#include "engine/engine_inline.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" @@ -751,8 +752,8 @@ int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum ma for (iter=0; iter < 500; iter++) { // make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec mju_quat2Mat(eigvec, quat); - mju_mulMatTMat3(tmp, eigvec, mat); - mju_mulMatMat3(D, tmp, eigvec); + mji_mulMatTMat3(tmp, eigvec, mat); + mji_mulMatMat3(D, tmp, eigvec); // assign eigenvalues eigval[0] = D[0]; diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index a20f0b7e..bfff1a35 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -17,6 +17,7 @@ #include #include +#include "engine/engine_inline.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" @@ -32,7 +33,7 @@ void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) { // null quat: copy vec else if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) { - mju_copy3(res, vec); + mji_copy3(res, vec); } // regular processing @@ -124,7 +125,7 @@ void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) { } speed /= dt; - mju_scl3(res, axis, speed); + mji_scl3(res, axis, speed); } @@ -132,11 +133,11 @@ void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) { void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) { // qdif = neg(qb)*qa mjtNum qneg[4], qdif[4]; - mju_negQuat(qneg, qb); - mju_mulQuat(qdif, qneg, qa); + mji_negQuat(qneg, qb); + mji_mulQuat(qdif, qneg, qa); // convert to 3D velocity - mju_quat2Vel(res, qdif, 1); + mji_quat2Vel(res, qdif, 1); } @@ -157,16 +158,16 @@ void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) { // regular processing else { - const mjtNum q00 = quat[0]*quat[0]; - const mjtNum q01 = quat[0]*quat[1]; - const mjtNum q02 = quat[0]*quat[2]; - const mjtNum q03 = quat[0]*quat[3]; - const mjtNum q11 = quat[1]*quat[1]; - const mjtNum q12 = quat[1]*quat[2]; - const mjtNum q13 = quat[1]*quat[3]; - const mjtNum q22 = quat[2]*quat[2]; - const mjtNum q23 = quat[2]*quat[3]; - const mjtNum q33 = quat[3]*quat[3]; + mjtNum q00 = quat[0]*quat[0]; + mjtNum q01 = quat[0]*quat[1]; + mjtNum q02 = quat[0]*quat[2]; + mjtNum q03 = quat[0]*quat[3]; + mjtNum q11 = quat[1]*quat[1]; + mjtNum q12 = quat[1]*quat[2]; + mjtNum q13 = quat[1]*quat[3]; + mjtNum q22 = quat[2]*quat[2]; + mjtNum q23 = quat[2]*quat[3]; + mjtNum q33 = quat[3]*quat[3]; res[0] = q00 + q11 - q22 - q33; res[4] = q00 - q11 + q22 - q33; @@ -234,9 +235,9 @@ void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) { mjtNum angle, tmp[4], qrot[4]; // form local rotation quaternion, apply - mju_copy3(tmp, vel); + mji_copy3(tmp, vel); angle = scale * mju_normalize3(tmp); - mju_axisAngle2Quat(qrot, tmp, angle); + mji_axisAngle2Quat(qrot, tmp, angle); mju_normalize4(quat); mju_mulQuat(quat, quat, qrot); } @@ -256,7 +257,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) { } // compute angle and axis - mju_cross(axis, z, vn); + mji_cross(axis, z, vn); a = mju_normalize3(axis); // almost parallel @@ -272,7 +273,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) { // make quaternion from angle and axis a = mju_atan2(a, mju_dot3(vn, z)); - mju_axisAngle2Quat(quat, axis, a); + mji_axisAngle2Quat(quat, axis, a); } @@ -295,11 +296,11 @@ int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) { mjtNum col2_rot[3] = {rot[1], rot[4], rot[7]}; mjtNum col3_rot[3] = {rot[2], rot[5], rot[8]}; mjtNum omega[3], vec1[3], vec2[3], vec3[3]; - mju_cross(vec1, col1_rot, col1_mat); - mju_cross(vec2, col2_rot, col2_mat); - mju_cross(vec3, col3_rot, col3_mat); - mju_add3(omega, vec1, vec2); - mju_addTo3(omega, vec3); + mji_cross(vec1, col1_rot, col1_mat); + mji_cross(vec2, col2_rot, col2_mat); + mji_cross(vec3, col3_rot, col3_mat); + mji_add3(omega, vec1, vec2); + mji_addTo3(omega, vec3); mju_scl3(omega, omega, 1.0 / (mju_abs(mju_dot3(col1_rot, col1_mat) + mju_dot3(col2_rot, col2_mat) + mju_dot3(col3_rot, col3_mat)) + mjMINVAL)); @@ -308,7 +309,7 @@ int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) { break; } mjtNum qrot[4]; - mju_axisAngle2Quat(qrot, omega, w); + mji_axisAngle2Quat(qrot, omega, w); mju_mulQuat(quat, qrot, quat); mju_normalize4(quat); } @@ -323,22 +324,22 @@ void mju_mulPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos1[3], const mjtNum quat1[4], const mjtNum pos2[3], const mjtNum quat2[4]) { // quatres = quat1*quat2 - mju_mulQuat(quatres, quat1, quat2); + mji_mulQuat(quatres, quat1, quat2); mju_normalize4(quatres); // posres = quat1*pos2 + pos1 - mju_rotVecQuat(posres, pos2, quat1); - mju_addTo3(posres, pos1); + mji_rotVecQuat(posres, pos2, quat1); + mji_addTo3(posres, pos1); } // negate pose void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const mjtNum quat[4]) { // qres = neg(quat) - mju_negQuat(quatres, quat); + mji_negQuat(quatres, quat); // pres = -neg(quat)*pos - mju_rotVecQuat(posres, pos, quatres); + mji_rotVecQuat(posres, pos, quatres); mju_scl3(posres, posres, -1); } @@ -346,8 +347,8 @@ void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const // transform vector by pose void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], const mjtNum vec[3]) { // res = quat*vec + pos - mju_rotVecQuat(res, vec, quat); - mju_addTo3(res, pos); + mji_rotVecQuat(res, vec, quat); + mji_addTo3(res, pos); } @@ -397,7 +398,7 @@ void mju_crossForce(mjtNum res[6], const mjtNum vel[6], const mjtNum f[6]) { // express inertia in com-based frame -void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9], +void mju_inertCom(mjtNum* restrict res, const mjtNum inert[3], const mjtNum mat[9], const mjtNum dif[3], mjtNum mass) { // tmp = diag(inert) * mat' (mat is local-to-global rotation) mjtNum tmp[9] = {mat[0]*inert[0], mat[3]*inert[0], mat[6]*inert[0], @@ -442,17 +443,17 @@ void mju_mulInertVec(mjtNum* restrict res, const mjtNum i[10], const mjtNum v[6] // express motion axis in com-based frame -void mju_dofCom(mjtNum res[6], const mjtNum axis[3], const mjtNum offset[3]) { +void mju_dofCom(mjtNum* restrict res, const mjtNum axis[3], const mjtNum offset[3]) { // hinge if (offset) { - mju_copy3(res, axis); - mju_cross(res+3, axis, offset); + mji_copy3(res, axis); + mji_cross(res+3, axis, offset); } // slide else { mju_zero3(res); - mju_copy3(res+3, axis); + mji_copy3(res+3, axis); } } @@ -481,24 +482,24 @@ void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force, // apply translation mju_copy(tran, vec, 6); - mju_sub3(dif, newpos, oldpos); + mji_sub3(dif, newpos, oldpos); if (flg_force) { - mju_cross(cros, dif, vec+3); - mju_sub3(tran, vec, cros); + mji_cross(cros, dif, vec+3); + mji_sub3(tran, vec, cros); } else { - mju_cross(cros, dif, vec); - mju_sub3(tran+3, vec+3, cros); + mji_cross(cros, dif, vec); + mji_sub3(tran+3, vec+3, cros); } // if provided, apply old -> new rotation if (rotnew2old) { - mju_mulMatTVec3(res, rotnew2old, tran); - mju_mulMatTVec3(res+3, rotnew2old, tran+3); + mji_mulMatTVec3(res, rotnew2old, tran); + mji_mulMatTVec3(res+3, rotnew2old, tran+3); } // otherwise copy else { - mju_copy(res, tran, 6); + mji_copy6(res, tran); } } @@ -524,12 +525,12 @@ void mju_makeFrame(mjtNum frame[9]) { } // make yaxis orthogonal to xaxis - mju_scl3(tmp, frame, mju_dot3(frame, frame+3)); - mju_subFrom3(frame+3, tmp); + mji_scl3(tmp, frame, mju_dot3(frame, frame+3)); + mji_subFrom3(frame+3, tmp); mju_normalize3(frame+3); // zaxis = cross(xaxis, yaxis) - mju_cross(frame+6, frame, frame+3); + mji_cross(frame+6, frame, frame+3); } @@ -566,5 +567,5 @@ void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq) { } } - mju_copy4(quat, tmp); + mji_copy4(quat, tmp); }