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

Introduce private header engine_inline.h exploiting restrict and avoiding loops and copies in some commonly used utility functions.

Browse Source 
PiperOrigin-RevId: 843635464
Change-Id: I3b0553eea98424ccc7def77e3769e2994f3e9014
This commit is contained in:
Yuval Tassa
2025-12-12 04:48:21 -08:00
committed by Copybara-Service
parent a0a56065e0
commit 600f0f20bc
11 changed files with 964 additions and 487 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/CMakeLists.txt
+1
View File
@@ -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
src/engine/engine_collision_box.c
+118 -117
View File
@@ -15,6 +15,7 @@
#include <math.h>
#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;
src/engine/engine_collision_convex.c
+82 -83
View File
@@ -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;
}
src/engine/engine_collision_primitive.c
+70 -68
View File
@@ -16,6 +16,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#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;
src/engine/engine_core_smooth.c
+113 -107
View File
@@ -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
src/engine/engine_core_util.c
+20 -20
View File
@@ -18,6 +18,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#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++;
}
src/engine/engine_inline.h
+464
View File
@@ -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 <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#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_
src/engine/engine_passive.c
+42 -41
View File
@@ -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]);
src/engine/engine_support.c
+1
View File
@@ -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"
src/engine/engine_util_solve.c
+3 -2
View File
@@ -19,6 +19,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjsan.h> // 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];
src/engine/engine_util_spatial.c
+50 -49
View File
@@ -17,6 +17,7 @@
#include <math.h>
#include <mujoco/mjmodel.h>
#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);
}