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_inverse.h
engine_init.c engine_init.c
engine_init.h engine_init.h
engine_inline.h
engine_island.c engine_island.c
engine_island.h engine_island.h
engine_io.c engine_io.c
src/engine/engine_collision_box.c
+118 -117
View File
@@ -15,6 +15,7 @@
#include <math.h> #include <math.h>
#include "engine/engine_collision_primitive.h" #include "engine/engine_collision_primitive.h"
#include "engine/engine_inline.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
// hard-clamp vector to range [-limit(i), +limit(i)] // hard-clamp vector to range [-limit(i), +limit(i)]
@@ -43,14 +44,14 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
mjtNum pos[3]; mjtNum pos[3];
mjtNum dist, closest; mjtNum dist, closest;
mju_sub3(tmp, pos1, pos2); mji_sub3(tmp, pos1, pos2);
mju_mulMatTVec3(center, mat2, tmp); mji_mulMatTVec3(center, mat2, tmp);
mju_copy(clamped, center, 3); mji_copy3(clamped, center);
mju_clampVec(clamped, size2, 3); mju_clampVec(clamped, size2, 3);
mju_copy(deepest, center, 3); mji_copy3(deepest, center);
mju_sub3(tmp, clamped, center); mji_sub3(tmp, clamped, center);
dist = mju_normalize3(tmp); dist = mju_normalize3(tmp);
if (dist - size1[0] > margin) if (dist - size1[0] > margin)
@@ -70,20 +71,20 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
mjtNum nearest[3] = {0}; mjtNum nearest[3] = {0};
nearest[k / 2] = (k % 2 ? -1 : 1); nearest[k / 2] = (k % 2 ? -1 : 1);
mju_copy3(pos, center); mji_copy3(pos, center);
mju_addToScl3(pos, nearest, (size1[0] - closest) / 2); mji_addToScl3(pos, nearest, (size1[0] - closest) / 2);
mju_mulMatVec3(con[0].frame, mat2, nearest); mji_mulMatVec3(con[0].frame, mat2, nearest);
dist = -closest; dist = -closest;
} else { } else {
mju_addToScl3(deepest, tmp, size1[0]); mji_addToScl3(deepest, tmp, size1[0]);
mju_zero3(pos); mju_zero3(pos);
mju_addToScl3(pos, clamped, 0.5); mji_addToScl3(pos, clamped, 0.5);
mju_addToScl3(pos, deepest, 0.5); mji_addToScl3(pos, deepest, 0.5);
mju_mulMatVec3(con[0].frame, mat2, tmp); mji_mulMatVec3(con[0].frame, mat2, tmp);
} }
mju_mulMatVec3(tmp, mat2, pos); mji_mulMatVec3(tmp, mat2, pos);
mju_add3(con[0].pos, tmp, pos2); mji_add3(con[0].pos, tmp, pos2);
con[0].dist = dist - size1[0]; con[0].dist = dist - size1[0];
mju_zero3(con[0].frame + 3); mju_zero3(con[0].frame + 3);
@@ -147,15 +148,15 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
halflength = size1[1]; halflength = size1[1];
secondpos = -4; // initialize to no 2nd contact (valid values are between -1 and 1) secondpos = -4; // initialize to no 2nd contact (valid values are between -1 and 1)
mju_sub3(tmp1, pos1, pos2); // bring capsule to box-local frame (center's box is at (0,0,0)) mji_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_mulMatTVec3(pos, mat2, tmp1); // and axis parralel to world
tmp1[0] = mat1[2]; // capsule's axis tmp1[0] = mat1[2]; // capsule's axis
tmp1[1] = mat1[5]; tmp1[1] = mat1[5];
tmp1[2] = mat1[8]; tmp1[2] = mat1[8];
mju_mulMatTVec3(axis, mat2, tmp1); // do the same for the capsule axis mji_mulMatTVec3(axis, mat2, tmp1); // do the same for the capsule axis
mju_scl3(halfaxis, axis, halflength); // scale to get actual capsule half-axis mji_scl3(halfaxis, axis, halflength); // scale to get actual capsule half-axis
axisdir = 0; axisdir = 0;
if (halfaxis[0] > 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 // test to see if maybe the a face of the box is closest to the capsule
for (i = -1; i <= 1; i += 2) { for (i = -1; i <= 1; i += 2) {
mju_copy3(tmp1, pos); mji_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, i); mji_addToScl3(tmp1, halfaxis, i);
mju_copy3(tmp2, tmp1); mji_copy3(tmp2, tmp1);
for (c1 = 0, j = 0, c2 = -1; j < 3; j++) { for (c1 = 0, j = 0, c2 = -1; j < 3; j++) {
if (tmp1[j] < -size2[j]) { if (tmp1[j] < -size2[j]) {
@@ -196,7 +197,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
if (c1 > 1) if (c1 > 1)
continue; continue;
mju_subFrom3(tmp1, tmp2); mji_subFrom3(tmp1, tmp2);
dist = mju_dot3(tmp1, tmp1); dist = mju_dot3(tmp1, tmp1);
if (dist < bestdist) { if (dist < bestdist) {
@@ -225,7 +226,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
// find closest point between capsule and the edge // find closest point between capsule and the edge
mju_sub3(dif, tmp3, pos); mji_sub3(dif, tmp3, pos);
ma = size2[j] * size2[j]; ma = size2[j] * size2[j];
mb = -size2[j] * halfaxis[j]; mb = -size2[j] * halfaxis[j];
@@ -275,9 +276,9 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
x1 = -1, s1 = 0; 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; dif[j] += size2[j] * x1;
tmp1[2] = mju_dot3(dif, dif); tmp1[2] = mju_dot3(dif, dif);
@@ -546,8 +547,8 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
secondpos = 2; secondpos = 2;
mju_copy3(tmp1, pos); mji_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, -mul); mji_addToScl3(tmp1, halfaxis, -mul);
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
if (i != clface) { if (i != clface) {
@@ -570,7 +571,7 @@ skip:
// create sphere in original orientation at first contact point // create sphere in original orientation at first contact point
mju_copy3(tmp1, pos); mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, bestsegmentpos); mji_addToScl3(tmp1, halfaxis, bestsegmentpos);
mju_mulMatVec3(tmp2, mat2, tmp1); mju_mulMatVec3(tmp2, mat2, tmp1);
mju_addTo3(tmp2, pos2); mju_addTo3(tmp2, pos2);
@@ -580,7 +581,7 @@ skip:
if (secondpos > -3) { // secondpos was modified if (secondpos > -3) { // secondpos was modified
mju_copy3(tmp1, pos); mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation mji_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation
mju_mulMatVec3(tmp2, mat2, tmp1); mju_mulMatVec3(tmp2, mat2, tmp1);
mju_addTo3(tmp2, pos2); mju_addTo3(tmp2, pos2);
n += mjraw_SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2); 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; code = -1;
margin2 = margin * margin; margin2 = margin * margin;
mju_sub3(tmp1, pos2, pos1); mji_sub3(tmp1, pos2, pos1);
mju_mulMatTVec3(pos21, mat1, tmp1); mji_mulMatTVec3(pos21, mat1, tmp1);
mju_sub3(tmp1, pos1, pos2); mji_sub3(tmp1, pos1, pos2);
mju_mulMatTVec3(pos12, mat2, tmp1); mji_mulMatTVec3(pos12, mat2, tmp1);
mju_mulMatTMat3(rot, mat1, mat2); mju_mulMatTMat3(rot, mat1, mat2);
mju_transpose(rott, rot, 3, 3); 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++) for (i = 0; i < 9; i++)
rottabs[i] = mju_abs(rott[i]); rottabs[i] = mju_abs(rott[i]);
mju_mulMatVec3(plen2, rotabs, size2); mji_mulMatVec3(plen2, rotabs, size2);
mju_mulMatTVec3(plen1, rotabs, size1); mji_mulMatTVec3(plen1, rotabs, size1);
for (i = 0, penetration = margin; i < 3; i++) for (i = 0, penetration = margin; i < 3; i++)
penetration += size1[i] * 3 + size2[i] * 3; penetration += size1[i] * 3 + size2[i] * 3;
@@ -716,7 +717,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
cle2 += 1 << k; cle2 += 1 << k;
code = 12 + i * 3 + j; code = 12 + i * 3 + j;
mju_copy3(clnorm, tmp2); mji_copy3(clnorm, tmp2);
in = c2 < 0; 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) \ #define rotmatx(matres, matin) \
{ \ { \
mju_scl3(matres+0, matin+i0*3, f0); \ mji_scl3(matres+0, matin+i0*3, f0); \
mju_scl3(matres+3, matin+i1*3, f1); \ mji_scl3(matres+3, matin+i1*3, f1); \
mju_scl3(matres+6, matin+i2*3, f2); \ mji_scl3(matres+6, matin+i2*3, f2); \
} }
if (q2) { 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(p, pos12);
rotaxis(tmp1, size2); rotaxis(tmp1, size2);
mju_copy3(s, size1); mji_copy3(s, size1);
} else { } else {
// mju_mulMatMat(r,rotmore,rot,3,3,3); // 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(p, pos21);
rotaxis(tmp1, size1); rotaxis(tmp1, size1);
mju_copy3(s, size2); mji_copy3(s, size2);
} }
mju_transpose(rt, r, 3, 3); 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]; hz = ss[2];
p[2] -= hz; p[2] -= hz;
mju_copy3(lp, p); mji_copy3(lp, p);
for (clcorner = 0, i = 0; i < 3; i++) for (clcorner = 0, i = 0; i < 3; i++)
if (r[6 + i] < 0) if (r[6 + i] < 0)
clcorner += 1 << i; clcorner += 1 << i;
mju_addToScl3(lp, rt + 0, s[0] * ((clcorner & 1) ? 1 : -1)); mji_addToScl3(lp, rt + 0, s[0] * ((clcorner & 1) ? 1 : -1));
mju_addToScl3(lp, rt + 3, s[1] * ((clcorner & 2) ? 1 : -1)); mji_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 + 6, s[2] * ((clcorner & 4) ? 1 : -1));
m = k = 0; m = k = 0;
mju_copy3(pts[m++], lp); mji_copy3(pts[m++], lp);
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
if (mju_abs(r[6 + i]) < 0.5) if (mju_abs(r[6 + i]) < 0.5)
mju_scl3(pts[m++], rt + 3 * i, s[i] * ((clcorner & (1 << i)) ? -2 : 2)); mju_scl3(pts[m++], rt + 3 * i, s[i] * ((clcorner & (1 << i)) ? -2 : 2));
mju_add3(pts[3], pts[0], pts[1]); mji_add3(pts[3], pts[0], pts[1]);
mju_add3(pts[4], pts[0], pts[2]); mji_add3(pts[4], pts[0], pts[2]);
mju_add3(pts[5], pts[3], pts[2]); mji_add3(pts[5], pts[3], pts[2]);
if (m > 1) if (m > 1)
{ {
mju_copy3(lines[k] + 0, pts[0]); mji_copy3(lines[k] + 0, pts[0]);
mju_copy3(lines[k++] + 3, pts[1]); mji_copy3(lines[k++] + 3, pts[1]);
} }
if (m > 2) if (m > 2)
{ {
mju_copy3(lines[k] + 0, pts[0]); mji_copy3(lines[k] + 0, pts[0]);
mju_copy3(lines[k++] + 3, pts[2]); mji_copy3(lines[k++] + 3, pts[2]);
mju_copy3(lines[k] + 0, pts[3]); mji_copy3(lines[k] + 0, pts[3]);
mju_copy3(lines[k++] + 3, pts[2]); mji_copy3(lines[k++] + 3, pts[2]);
mju_copy3(lines[k] + 0, pts[4]); mji_copy3(lines[k] + 0, pts[4]);
mju_copy3(lines[k++] + 3, pts[1]); mji_copy3(lines[k++] + 3, pts[1]);
} }
for (i = 0; i < k; i++) { 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]) if (mju_abs(c2) > ss[1 - q])
continue; continue;
mju_copy3(points[n], lines[i]); mji_copy3(points[n], lines[i]);
mju_addToScl3(points[n++], lines[i] + 3, c1); 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++) { 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) 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) if (tmp1[1] <= -ly || tmp1[1] >= ly)
continue; 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) if (points[i][2] > margin)
continue; continue;
mju_copy3(points[n], points[i]); mji_copy3(points[n], points[i]);
depth[n] = points[n][2]; depth[n] = points[n][2];
points[n][2] *= 0.5; 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[1] = (q2 ? -1 : 1) * r[5];
tmp2[2] = (q2 ? -1 : 1) * r[8]; 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); 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]; con[i].dist = 2 * points[i][2];
points[i][2] += hz; points[i][2] += hz;
mju_mulMatVec3(tmp2, r, points[i]); mji_mulMatVec3(tmp2, r, points[i]);
mju_add3(con[i].pos, tmp2, p); mji_add3(con[i].pos, tmp2, p);
if (i) 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); // mju_mulMatMat(r,rotmore,rot,3,3,3);
rotmatx(r, rot); rotmatx(r, rot);
mju_mulMatTVec3(tmp1, rotmore, size1); mji_mulMatTVec3(tmp1, rotmore, size1);
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
s[i] = mju_abs(tmp1[i]); s[i] = mju_abs(tmp1[i]);
@@ -1092,29 +1093,29 @@ edgeedge:
n = 0; n = 0;
mju_copy3(points[n], p); mji_copy3(points[n], p);
mju_addToScl3(points[n], rt + 3 * ax1, size2[ax1] * ((cle2 & (1 << ax1)) ? 1 : -1)); mji_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)); mji_addToScl3(points[n], rt + 3 * ax2, size2[ax2] * ((cle2 & (1 << ax2)) ? 1 : -1));
mju_copy3(points[n + 1], points[n]); mji_copy3(points[n + 1], points[n]);
mju_addToScl3(points[n], rt + 3 * q2, size2[q2]); mji_addToScl3(points[n], rt + 3 * q2, size2[q2]);
n = 1; n = 1;
mju_addToScl3(points[n], rt + 3 * q2, -size2[q2]); mji_addToScl3(points[n], rt + 3 * q2, -size2[q2]);
n = 2; n = 2;
mju_copy3(points[n], p); mji_copy3(points[n], p);
mju_addToScl3(points[n], rt + 3 * ax1, size2[ax1] * ((cle2 & (1 << ax1)) ? -1 : 1)); mji_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)); mji_addToScl3(points[n], rt + 3 * ax2, size2[ax2] * ((cle2 & (1 << ax2)) ? 1 : -1));
mju_copy3(points[n + 1], points[n]); mji_copy3(points[n + 1], points[n]);
mju_addToScl3(points[n], rt + 3 * q2, size2[q2]); mji_addToScl3(points[n], rt + 3 * q2, size2[q2]);
n = 3; n = 3;
mju_addToScl3(points[n], rt + 3 * q2, -size2[q2]); mji_addToScl3(points[n], rt + 3 * q2, -size2[q2]);
n = 4; n = 4;
mju_copy3(axi[0], points[0]); mji_copy3(axi[0], points[0]);
mju_sub3(axi[1], points[1], points[0]); mji_sub3(axi[1], points[1], points[0]);
mju_sub3(axi[2], points[2], points[0]); mji_sub3(axi[2], points[2], points[0]);
if (mju_abs(rnorm[2]) < mjMINVAL) if (mju_abs(rnorm[2]) < mjMINVAL)
@@ -1127,9 +1128,9 @@ edgeedge:
{ {
c1 = -points[i][2] * (1 / rnorm[2]); 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][0]=points[i][0];
// ppts[i][1]=points[i][1]; // ppts[i][1]=points[i][1];
@@ -1138,9 +1139,9 @@ edgeedge:
} }
mju_copy3(pts[0], points[0]); mji_copy3(pts[0], points[0]);
mju_sub3(pts[1], points[1], points[0]); mji_sub3(pts[1], points[1], points[0]);
mju_sub3(pts[2], points[2], points[0]); mji_sub3(pts[2], points[2], points[0]);
m = 3; m = 3;
k = 0; k = 0;
@@ -1148,26 +1149,26 @@ edgeedge:
if (m > 1) { if (m > 1) {
mju_copy3(lines[k] + 0, pts[0]); mji_copy3(lines[k] + 0, pts[0]);
mju_copy3(lines[k] + 3, pts[1]); mji_copy3(lines[k] + 3, pts[1]);
mju_copy3(linesu[k] + 0, axi[0]); mji_copy3(linesu[k] + 0, axi[0]);
mju_copy3(linesu[k++] + 3, axi[1]); mji_copy3(linesu[k++] + 3, axi[1]);
} }
if (m > 2) { if (m > 2) {
mju_copy3(lines[k] + 0, pts[0]); mji_copy3(lines[k] + 0, pts[0]);
mju_copy3(lines[k] + 3, pts[2]); mji_copy3(lines[k] + 3, pts[2]);
mju_copy3(linesu[k] + 0, axi[0]); mji_copy3(linesu[k] + 0, axi[0]);
mju_copy3(linesu[k++] + 3, axi[2]); mji_copy3(linesu[k++] + 3, axi[2]);
mju_add3(lines[k] + 0, pts[0], pts[1]); mji_add3(lines[k] + 0, pts[0], pts[1]);
mju_copy3(lines[k] + 3, pts[2]); mji_copy3(lines[k] + 3, pts[2]);
mju_add3(linesu[k] + 0, axi[0], axi[1]); mji_add3(linesu[k] + 0, axi[0], axi[1]);
mju_copy3(linesu[k++] + 3, axi[2]); mji_copy3(linesu[k++] + 3, axi[2]);
mju_add3(lines[k] + 0, pts[0], pts[2]); mji_add3(lines[k] + 0, pts[0], pts[2]);
mju_copy3(lines[k] + 3, pts[1]); mji_copy3(lines[k] + 3, pts[1]);
mju_add3(linesu[k] + 0, axi[0], axi[2]); mji_add3(linesu[k] + 0, axi[0], axi[2]);
mju_copy3(linesu[k++] + 3, axi[1]); mji_copy3(linesu[k++] + 3, axi[1]);
} }
for (i = 0; i < k; i++) { for (i = 0; i < k; i++) {
@@ -1191,8 +1192,8 @@ edgeedge:
if ((linesu[i][2] + linesu[i][5]*c1)*innorm > margin) if ((linesu[i][2] + linesu[i][5]*c1)*innorm > margin)
continue; continue;
mju_scl3(points[n], linesu[i], 0.5); mji_scl3(points[n], linesu[i], 0.5);
mju_addToScl3(points[n], linesu[i] + 3, 0.5 * c1); mji_addToScl3(points[n], linesu[i] + 3, 0.5 * c1);
points[n][0 + q] += 0.5 * l; points[n][0 + q] += 0.5 * l;
points[n][1 - q] += 0.5 * c2; points[n][1 - q] += 0.5 * c2;
depth[n] = points[n][2] * innorm * 2; depth[n] = points[n][2] * innorm * 2;
@@ -1240,22 +1241,22 @@ edgeedge:
v = 1; v = 1;
mju_scl3(tmp1, pu[0], 1 - u - v); mji_scl3(tmp1, pu[0], 1 - u - v);
mju_addToScl3(tmp1, pu[1], u); mji_addToScl3(tmp1, pu[1], u);
mju_addToScl3(tmp1, pu[2], v); mji_addToScl3(tmp1, pu[2], v);
points[n][0] = llx; points[n][0] = llx;
points[n][1] = lly; points[n][1] = lly;
points[n][2] = 0; points[n][2] = 0;
mju_sub3(tmp2, points[n], tmp1); mji_sub3(tmp2, points[n], tmp1);
c1 = mju_dot3(tmp2, tmp2); c1 = mju_dot3(tmp2, tmp2);
if (tmp1[2] > 0) if (tmp1[2] > 0)
if (c1 > margin2) if (c1 > margin2)
continue; continue;
mju_add3(points[n], points[n], tmp1); mji_add3(points[n], points[n], tmp1);
mju_scl3(points[n], points[n], 0.5); mju_scl3(points[n], points[n], 0.5);
depth[n] = sqrt(c1) * (tmp1[2] < 0 ? -1 : 1); depth[n] = sqrt(c1) * (tmp1[2] < 0 ? -1 : 1);
@@ -1305,8 +1306,8 @@ edgeedge:
else if (ppts2[i][j] > s[j]) else if (ppts2[i][j] > s[j])
tmp1[j] = +s[j] * 0.5; tmp1[j] = +s[j] * 0.5;
} }
mju_addToScl3(tmp1, pu[i], 0.5); mji_addToScl3(tmp1, pu[i], 0.5);
mju_copy3(points[n], tmp1); mji_copy3(points[n], tmp1);
depth[n] = sqrt(c1) * (pu[i][2] < 0 ? -1 : 1); depth[n] = sqrt(c1) * (pu[i][2] < 0 ? -1 : 1);
n++; n++;
@@ -1315,9 +1316,9 @@ edgeedge:
mju_mulMatMatT3(r, mat1, rotmore); 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); mju_zero3(con[0].frame + 3);
@@ -1325,11 +1326,11 @@ edgeedge:
con[i].dist = depth[i]; con[i].dist = depth[i];
points[i][2] += hz; 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; 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_gjk.h"
#include "engine/engine_collision_primitive.h" #include "engine/engine_collision_primitive.h"
#include "engine/engine_memory.h" #include "engine/engine_memory.h"
#include "engine/engine_inline.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h" #include "engine/engine_util_misc.h"
@@ -99,17 +100,17 @@ void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
// return geom position // return geom position
if (g >= 0) { 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 // return flex element position
else if (e >= 0) { 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 // return flex vertex position
else { 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 // compute mean
mju_zero3(res); mju_zero3(res);
for (int i=0; i < 6; i++) { 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); 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 // point support function
void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjtNum* pos = obj->data->geom_xpos + 3*obj->geom; const mjtNum* pos = obj->data->geom_xpos + 3*obj->geom;
res[0] = pos[0]; mji_copy3(res, pos);
res[1] = pos[1];
res[2] = pos[2];
} }
@@ -448,7 +447,7 @@ static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3])
} }
// copy best point // 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]; const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
// find element vertex with largest projection along dir // 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); mjtNum best = mju_dot3(res, dir);
for (int i=1; i <= dim; i++) { for (int i=1; i <= dim; i++) {
mjtNum dot = mju_dot3(vert+3*edata[i], dir); 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 // better vertex found: assign
if (dot > best) { if (dot > best) {
best = dot; best = dot;
mju_copy3(res, vert+3*edata[i]); mji_copy3(res, vert+3*edata[i]);
} }
} }
// add radius and margin/2 // 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; return;
} }
// flex vertex // flex vertex
else { else {
const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert); 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; 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]; const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
// find element vertex with largest projection along dir // 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); mjtNum best = mju_dot3(res, dir);
for (int i=1; i <= dim; i++) { for (int i=1; i <= dim; i++) {
mjtNum dot = mju_dot3(vert+3*edata[i], dir); 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 // better vertex found: assign
if (dot > best) { if (dot > best) {
best = dot; best = dot;
mju_copy3(res, vert+3*edata[i]); mji_copy3(res, vert+3*edata[i]);
} }
} }
// add radius and margin/2 // 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; return;
} }
// flex vertex // flex vertex
else { else {
const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert); 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; 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 // compute result according to geom type
switch ((mjtGeom) obj->geom_type) { switch ((mjtGeom) obj->geom_type) {
case mjGEOM_SPHERE: case mjGEOM_SPHERE:
mju_scl3(res, local_dir, size[0]); mji_scl3(res, local_dir, size[0]);
break; break;
case mjGEOM_CAPSULE: case mjGEOM_CAPSULE:
// start with sphere // start with sphere
mju_scl3(res, local_dir, size[0]); mji_scl3(res, local_dir, size[0]);
// add cylinder contribution // add cylinder contribution
res[2] += mju_sign(local_dir[2]) * size[1]; 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); mju_mulMatVec3(res, d->geom_xmat+9*g, res);
// add geom position // 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 // fill in contact data
con->dist = margin-depth; con->dist = margin-depth;
mju_copy3(con->frame, dir.v); mji_copy3(con->frame, dir.v);
mju_copy3(con->pos, pos.v); mji_copy3(con->pos, pos.v);
mju_zero3(con->frame+3); mju_zero3(con->frame+3);
// both geoms: fix contact frame normal // 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); mju_copy(xmat, mat, 9);
// vector to rotation origin: rel = origin - xpos // 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 // displacement of origin due to rotation: vec = rot*rel - rel
mju_mulMatVec3(vec, rot, rel); mju_mulMatVec3(vec, rot, rel);
mju_subFrom3(vec, rel); mju_subFrom3(vec, rel);
// correct xpos by subtracting displacement: xpos = xpos - vec // 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 // save positions and orientations of g1 and g2
mjtNum xpos1[3], xmat1[9], xpos2[3], xmat2[9]; mjtNum xpos1[3], xmat1[9], xpos2[3], xmat2[9];
mju_copy3(xpos1, d->geom_xpos+3*g1); mji_copy3(xpos1, d->geom_xpos+3*g1);
mju_copy(xmat1, d->geom_xmat+9*g1, 9); mji_copy9(xmat1, d->geom_xmat+9*g1);
mju_copy3(xpos2, d->geom_xpos+3*g2); mji_copy3(xpos2, d->geom_xpos+3*g2);
mju_copy(xmat2, d->geom_xmat+9*g2, 9); mji_copy9(xmat2, d->geom_xmat+9*g2);
// complete frame of initial contact // complete frame of initial contact
mjtNum frame[9]; mjtNum frame[9];
mju_copy(frame, con[0].frame, 9); mji_copy9(frame, con[0].frame);
mju_makeFrame(frame); mju_makeFrame(frame);
// tolerance for determining if newly found contacts are distinct // 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 // make rotation matrix rot
mjtNum quat[4], rot[9]; mjtNum quat[4], rot[9];
mju_axisAngle2Quat(quat, axis, angle); mji_axisAngle2Quat(quat, axis, angle);
mju_quat2Mat(rot, quat); mju_quat2Mat(rot, quat);
// rotate g1 around initial contact point // 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 // reset positions and orientations of g1 and g2
mju_copy3(d->geom_xpos+3*g1, xpos1); mji_copy3(d->geom_xpos+3*g1, xpos1);
mju_copy(d->geom_xmat+9*g1, xmat1, 9); mji_copy9(d->geom_xmat+9*g1, xmat1);
mju_copy3(d->geom_xpos+3*g2, xpos2); mji_copy3(d->geom_xpos+3*g2, xpos2);
mju_copy(d->geom_xmat+9*g2, xmat2, 9); 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 // pnt-pos difference vector
mjtNum dif[3]; mjtNum dif[3];
mju_sub3(dif, pnt, pos1); mji_sub3(dif, pnt, pos1);
// set distance // set distance
con->dist = mju_dot3(normal1, dif); con->dist = mju_dot3(normal1, dif);
// set position // set position
mju_copy3(con->pos, pnt); mji_copy3(con->pos, pnt);
mju_addToScl3(con->pos, normal1, -0.5*con->dist); mji_addToScl3(con->pos, normal1, -0.5*con->dist);
// set frame // set frame
mju_copy3(con->frame, normal1); mji_copy3(con->frame, normal1);
mju_zero3(con->frame+3); mju_zero3(con->frame+3);
return 1; return 1;
@@ -1052,7 +1051,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
mjccd_support(&obj, &dir, &vec); mjccd_support(&obj, &dir, &vec);
// compute normal distance, return if too far // compute normal distance, return if too far
mju_sub3(dif, vec.v, pos1); mji_sub3(dif, vec.v, pos1);
dist = mju_dot3(normal, dif); dist = mju_dot3(normal, dif);
if (dist > margin) { if (dist > margin) {
return 0; return 0;
@@ -1060,9 +1059,9 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
// fill in contact data // fill in contact data
con->dist = dist; con->dist = dist;
mju_copy3(con->pos, vec.v); mji_copy3(con->pos, vec.v);
mju_addToScl3(con->pos, normal, -0.5*dist); mji_addToScl3(con->pos, normal, -0.5*dist);
mju_copy3(con->frame, normal); mji_copy3(con->frame, normal);
mju_zero3(con->frame+3); mju_zero3(con->frame+3);
//--------------- add all/connected vertices below margin //--------------- 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); mju_mulMatTVec3(locdir, d->geom_xmat+9*g, dir.v);
// inclusion threshold along locdir, relative to geom2 center // 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; mjtNum threshold = mju_dot3(normal, dif) - margin;
// no graph data: exhaustive search // 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 // add vertex to prism
static inline void addVert(mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) { static inline void addVert(mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) {
// move old data // move old data
mju_copy3(obj->prism[0], obj->prism[1]); mji_copy3(obj->prism[0], obj->prism[1]);
mju_copy3(obj->prism[1], obj->prism[2]); mji_copy3(obj->prism[1], obj->prism[2]);
mju_copy3(obj->prism[3], obj->prism[4]); mji_copy3(obj->prism[3], obj->prism[4]);
mju_copy3(obj->prism[4], obj->prism[5]); mji_copy3(obj->prism[4], obj->prism[5]);
// add new vertex at last position // add new vertex at last position
obj->prism[2][0] = obj->prism[5][0] = x; 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 // add vertex to prism
static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, mjtNum dy, mjtNum margin) { static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, mjtNum dy, mjtNum margin) {
// move old data // move old data
mju_copy3(obj->prism[0], obj->prism[1]); mji_copy3(obj->prism[0], obj->prism[1]);
mju_copy3(obj->prism[1], obj->prism[2]); mji_copy3(obj->prism[1], obj->prism[2]);
mju_copy3(obj->prism[3], obj->prism[4]); mji_copy3(obj->prism[3], obj->prism[4]);
mju_copy3(obj->prism[4], obj->prism[5]); mji_copy3(obj->prism[4], obj->prism[5]);
int dr = 1 - i; int dr = 1 - i;
@@ -1226,14 +1225,14 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// express geom2 mat in hfield frame // express geom2 mat in hfield frame
mjtNum mat[9]; mjtNum mat[9];
mju_mulMatTMat3(mat, mat1, mat2); mji_mulMatTMat3(mat, mat1, mat2);
// save mat2 and pos2, replace with relative frame // save mat2 and pos2, replace with relative frame
mjtNum savemat2[9], savepos2[3]; mjtNum savemat2[9], savepos2[3];
mju_copy9(savemat2, mat2); mji_copy9(savemat2, mat2);
mju_copy3(savepos2, pos2); mji_copy3(savepos2, pos2);
mju_copy9(mat2, mat); mji_copy9(mat2, mat);
mju_copy3(pos2, pos); mji_copy3(pos2, pos);
mjtNum dir[3] = {0, 0, 0}, res[3]; 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) || if ((xmin - margin > size0) || (xmax + margin < -size0) ||
(ymin - margin > size1) || (ymax + margin < -size1) || (ymin - margin > size1) || (ymax + margin < -size1) ||
(zmin - margin > size2) || (zmax + margin < -size3)) { (zmin - margin > size2) || (zmax + margin < -size3)) {
mju_copy9(mat2, savemat2); mji_copy9(mat2, savemat2);
mju_copy3(pos2, savepos2); mji_copy3(pos2, savepos2);
return 0; return 0;
} }
@@ -1332,9 +1331,9 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
&& !ccdVec3Eq(&dirccd, ccd_vec3_origin)) { && !ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates // fill in contact data, transform to global coordinates
con[ncon].dist = -depth; con[ncon].dist = -depth;
mju_mulMatVec3(con[ncon].frame, mat1, dirccd.v); mji_mulMatVec3(con[ncon].frame, mat1, dirccd.v);
mju_mulMatVec3(con[ncon].pos, mat1, vecccd.v); mji_mulMatVec3(con[ncon].pos, mat1, vecccd.v);
mju_addTo3(con[ncon].pos, pos1); mji_addTo3(con[ncon].pos, pos1);
mju_zero3(con[ncon].frame+3); mju_zero3(con[ncon].frame+3);
// force out of all loops if max contacts reached // 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 // restore mat2 and pos2
mju_copy9(mat2, savemat2); mji_copy9(mat2, savemat2);
mju_copy3(pos2, savepos2); mji_copy3(pos2, savepos2);
if (mjDISABLED(mjDSBL_NATIVECCD)) { if (mjDISABLED(mjDSBL_NATIVECCD)) {
// fix contact normals // fix contact normals
@@ -1401,7 +1400,7 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum
// new point on ellipsoid // new point on ellipsoid
mjtNum pnt[3]; mjtNum pnt[3];
mju_addScl3(pnt, pos, nrm, x); mji_addScl3(pnt, pos, nrm, x);
// normal at new point // normal at new point
mjtNum newnrm[3] = {pnt[0]*S2inv[0], pnt[1]*S2inv[1], pnt[2]*S2inv[2]}; 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 // save change and assign
mjtNum change = mju_dist3(nrm, newnrm); mjtNum change = mju_dist3(nrm, newnrm);
mju_copy3(nrm, newnrm); mji_copy3(nrm, newnrm);
// terminate if converged // terminate if converged
if (change < tolerance) { 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) { void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2) {
mjtNum dst1, dst2; 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 // process according to type
switch (type[i]) { switch (type[i]) {
case mjGEOM_SPHERE: case mjGEOM_SPHERE:
mju_copy3(nrm, pos); mji_copy3(nrm, pos);
processed[i] = 1; processed[i] = 1;
break; 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 // normalize and map normal to global frame
if (processed[i]) { if (processed[i]) {
mju_normalize3(nrm); mju_normalize3(nrm);
mju_mulMatVec3(normal[i], mat, nrm); mji_mulMatVec3(normal[i], mat, nrm);
} }
} }
} }
// both processed: average // both processed: average
if (processed[0] && processed[1]) { 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); mju_normalize3(con->frame);
} }
// first processed: copy // first processed: copy
else if (processed[0]) { else if (processed[0]) {
mju_copy3(con->frame, normal[0]); mji_copy3(con->frame, normal[0]);
} }
// second processed: copy reverse // second processed: copy reverse
else if (processed[1]) { 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 // 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 // save elem vertices, transform to hfield frame
mjtNum savevert[4][3]; mjtNum savevert[4][3];
for (int i=0; i <= dim; i++) { for (int i=0; i <= dim; i++) {
mju_copy3(savevert[i], evert[i]); mji_copy3(savevert[i], evert[i]);
mju_sub3(vec, evert[i], hpos); mji_sub3(vec, evert[i], hpos);
mju_mulMatTVec(evert[i], hmat, vec, 3, 3); mji_mulMatTVec3(evert[i], hmat, vec);
} }
// save elem center, transform to hfield frame // save elem center, transform to hfield frame
mjtNum savecenter[3]; mjtNum savecenter[3];
mju_copy3(savecenter, ecenter); mji_copy3(savecenter, ecenter);
mju_sub3(vec, ecenter, hpos); mji_sub3(vec, ecenter, hpos);
mju_mulMatTVec(ecenter, hmat, vec, 3, 3); mji_mulMatTVec3(ecenter, hmat, vec);
// compute elem bounding box (in hfield frame) // compute elem bounding box (in hfield frame)
xmin = xmax = evert[0][0]; 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])) { (zmin-margin > hsize[2]) || (zmax+margin < -hsize[3])) {
// restore vertices and center // restore vertices and center
for (int i=0; i <= dim; i++) { 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; return 0;
} }
@@ -1768,9 +1767,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) { if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates // fill in contact data, transform to global coordinates
con[cnt].dist = -depth; con[cnt].dist = -depth;
mju_mulMatVec3(con[cnt].frame, hmat, dirccd.v); mji_mulMatVec3(con[cnt].frame, hmat, dirccd.v);
mju_mulMatVec3(con[cnt].pos, hmat, vecccd.v); mji_mulMatVec3(con[cnt].pos, hmat, vecccd.v);
mju_addTo3(con[cnt].pos, hpos); mji_addTo3(con[cnt].pos, hpos);
mju_zero3(con[cnt].frame+3); mju_zero3(con[cnt].frame+3);
// count, stop if max number reached // 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 // restore elem vertices and center
for (int i=0; i <= dim; i++) { 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; return cnt;
} }
src/engine/engine_collision_primitive.c
+70 -68
View File
@@ -16,6 +16,7 @@
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include "engine/engine_inline.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
#include "engine/engine_util_misc.h" #include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h" #include "engine/engine_util_spatial.h"
@@ -41,8 +42,8 @@ static int mjraw_PlaneSphere(mjContact* con, mjtNum margin,
// depth and position // depth and position
con[0].dist = cdist - size2[0]; con[0].dist = cdist - size2[0];
mju_scl3(tmp, con[0].frame, -con[0].dist/2 - size2[0]); mji_scl3(tmp, con[0].frame, -con[0].dist / 2 - size2[0]);
mju_add3(con[0].pos, pos2, tmp); mji_add3(con[0].pos, pos2, tmp);
mju_zero3(con[0].frame+3); mju_zero3(con[0].frame+3);
return 1; return 1;
@@ -77,10 +78,10 @@ int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
// align contact frames with capsule axis // align contact frames with capsule axis
if (n1) { if (n1) {
mju_copy3(con->frame+3, axis); mji_copy3(con->frame + 3, axis);
} }
if (n2) { if (n2) {
mju_copy3((con+n1)->frame+3, axis); mji_copy3((con + n1)->frame + 3, axis);
} }
return n1+n2; return n1+n2;
@@ -136,10 +137,10 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
int cnt = 0; int cnt = 0;
if (dist0 + prjaxis + prjvec <= margin) { if (dist0 + prjaxis + prjvec <= margin) {
con[cnt].dist = dist0 + prjaxis + prjvec; con[cnt].dist = dist0 + prjaxis + prjvec;
mju_add3(con[cnt].pos, pos2, vec); mji_add3(con[cnt].pos, pos2, vec);
mju_addTo3(con[cnt].pos, axis); mji_addTo3(con[cnt].pos, axis);
mju_addToScl3(con[cnt].pos, normal, -con[cnt].dist*0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
mju_copy3(con[cnt].frame, normal); mji_copy3(con[cnt].frame, normal);
mju_zero3(con[cnt].frame+3); mju_zero3(con[cnt].frame+3);
cnt++; cnt++;
} else { } else {
@@ -149,10 +150,10 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
// check second point, construct contact // check second point, construct contact
if (dist0 - prjaxis + prjvec <= margin) { if (dist0 - prjaxis + prjvec <= margin) {
con[cnt].dist = dist0 - prjaxis + prjvec; con[cnt].dist = dist0 - prjaxis + prjvec;
mju_add3(con[cnt].pos, pos2, vec); mji_add3(con[cnt].pos, pos2, vec);
mju_subFrom3(con[cnt].pos, axis); mji_subFrom3(con[cnt].pos, axis);
mju_addToScl3(con[cnt].pos, normal, -con[cnt].dist*0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
mju_copy3(con[cnt].frame, normal); mji_copy3(con[cnt].frame, normal);
mju_zero3(con[cnt].frame+3); mju_zero3(con[cnt].frame+3);
cnt++; cnt++;
} }
@@ -162,27 +163,27 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
if (dist0 + prjaxis + prjvec1 <= margin) { if (dist0 + prjaxis + prjvec1 <= margin) {
// compute sideways vector: vec1 // compute sideways vector: vec1
mjtNum vec1[3]; mjtNum vec1[3];
mju_cross(vec1, vec, axis); mji_cross(vec1, vec, axis);
mju_normalize3(vec1); 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 // add point A
con[cnt].dist = dist0 + prjaxis + prjvec1; con[cnt].dist = dist0 + prjaxis + prjvec1;
mju_add3(con[cnt].pos, pos2, vec1); mji_add3(con[cnt].pos, pos2, vec1);
mju_addTo3(con[cnt].pos, axis); mji_addTo3(con[cnt].pos, axis);
mju_addToScl3(con[cnt].pos, vec, -0.5); mji_addToScl3(con[cnt].pos, vec, -0.5);
mju_addToScl3(con[cnt].pos, normal, -con[cnt].dist*0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
mju_copy3(con[cnt].frame, normal); mji_copy3(con[cnt].frame, normal);
mju_zero3(con[cnt].frame+3); mju_zero3(con[cnt].frame+3);
cnt++; cnt++;
// add point B // add point B
con[cnt].dist = dist0 + prjaxis + prjvec1; con[cnt].dist = dist0 + prjaxis + prjvec1;
mju_sub3(con[cnt].pos, pos2, vec1); mji_sub3(con[cnt].pos, pos2, vec1);
mju_addTo3(con[cnt].pos, axis); mji_addTo3(con[cnt].pos, axis);
mju_addToScl3(con[cnt].pos, vec, -0.5); mji_addToScl3(con[cnt].pos, vec, -0.5);
mju_addToScl3(con[cnt].pos, normal, -con[cnt].dist*0.5); mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
mju_copy3(con[cnt].frame, normal); mji_copy3(con[cnt].frame, normal);
mju_zero3(con[cnt].frame+3); mju_zero3(con[cnt].frame+3);
cnt++; cnt++;
} }
@@ -222,11 +223,11 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d,
// construct contact // construct contact
con[cnt].dist = dist + ldist; con[cnt].dist = dist + ldist;
mju_copy3(con[cnt].frame, norm); mji_copy3(con[cnt].frame, norm);
mju_zero3(con[cnt].frame+3); mju_zero3(con[cnt].frame+3);
mju_addTo3(corner, pos2); mji_addTo3(corner, pos2);
mju_scl3(vec, norm, -con[cnt].dist/2); mji_scl3(vec, norm, -con[cnt].dist / 2);
mju_add3(con[cnt].pos, corner, vec); mji_add3(con[cnt].pos, corner, vec);
// count; max is 4 // count; max is 4
if (++cnt >= 4) { if (++cnt >= 4) {
@@ -254,7 +255,7 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
// depth and normal // depth and normal
con[0].dist = mju_sqrt(cdist_sqr) - size1[0] - size2[0]; 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); mjtNum len = mju_normalize3(con[0].frame);
// if centers are the same, norm = cross-product of z axes // 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) { if (len < mjMINVAL) {
mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum axis2[3] = {mat2[2], mat2[5], mat2[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); mju_normalize3(con[0].frame);
} }
// position // position
mju_scl3(con[0].pos, con[0].frame, size1[0] + con[0].dist/2); mji_scl3(con[0].pos, con[0].frame, size1[0] + con[0].dist / 2);
mju_addTo3(con[0].pos, pos1); mji_addTo3(con[0].pos, pos1);
mju_zero3(con[0].frame+3); mju_zero3(con[0].frame+3);
return 1; return 1;
@@ -296,8 +297,8 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
mjtNum x = mju_clip(mju_dot3(axis, vec), -len, len); mjtNum x = mju_clip(mju_dot3(axis, vec), -len, len);
// find nearest point on segment, do sphere-sphere test // find nearest point on segment, do sphere-sphere test
mju_scl3(vec, axis, x); mji_scl3(vec, axis, x);
mju_addTo3(vec, pos2); mji_addTo3(vec, pos2);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size2); 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 vec[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
mjtNum x = mju_dot3(axis, vec); mjtNum x = mju_dot3(axis, vec);
mjtNum a_proj[3], p_proj[3]; mjtNum a_proj[3], p_proj[3];
mju_scl3(a_proj, axis, x); mji_scl3(a_proj, axis, x);
mju_sub3(p_proj, vec, a_proj); mji_sub3(p_proj, vec, a_proj);
mjtNum p_proj_sqr = mju_dot3(p_proj, p_proj); mjtNum p_proj_sqr = mju_dot3(p_proj, p_proj);
// get collision type // get collision type
@@ -343,7 +344,7 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d,
// side collision: use sphere-sphere // side collision: use sphere-sphere
if (collide_side) { 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); 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 // otherwise corner collision: use sphere-sphere
mju_scl3(p_proj, p_proj, size2[0] / mju_sqrt(p_proj_sqr)); // denominator cannot be 0 mji_scl3(p_proj, p_proj,
mju_scl3(vec, axis, x > 0 ? height : -height); size2[0] / mju_sqrt(p_proj_sqr)); // denominator cannot be 0
mju_addTo3(vec, p_proj); mji_scl3(vec, axis, x > 0 ? height : -height);
mju_addTo3(vec, pos2); mji_addTo3(vec, p_proj);
mji_addTo3(vec, pos2);
// sphere-sphere with point sphere at the corner // sphere-sphere with point sphere at the corner
mjtNum size_zero[1] = {0}; mjtNum size_zero[1] = {0};
@@ -423,10 +425,10 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
// find nearest points, do sphere-sphere test // find nearest points, do sphere-sphere test
mjtNum vec1[3], vec2[3]; mjtNum vec1[3], vec2[3];
mju_scl3(vec1, axis1, x1); mji_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1); mji_addTo3(vec1, pos1);
mju_scl3(vec2, axis2, x2); mji_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2); mji_addTo3(vec2, pos2);
return mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2); return mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
} }
@@ -435,19 +437,19 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
else { else {
// x1 = 1 // x1 = 1
mjtNum vec1[3]; mjtNum vec1[3];
mju_add3(vec1, pos1, axis1); mji_add3(vec1, pos1, axis1);
mjtNum x2 = mju_clip((v - mb) / mc, -1, 1); mjtNum x2 = mju_clip((v - mb) / mc, -1, 1);
mjtNum vec2[3]; mjtNum vec2[3];
mju_scl3(vec2, axis2, x2); mji_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2); mji_addTo3(vec2, pos2);
int n1 = mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2); int n1 = mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
// x1 = -1 // x1 = -1
mju_sub3(vec1, pos1, axis1); mji_sub3(vec1, pos1, axis1);
x2 = mju_clip((v + mb) / mc, -1, 1); x2 = mju_clip((v + mb) / mc, -1, 1);
mju_scl3(vec2, axis2, x2); mji_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2); mji_addTo3(vec2, pos2);
int n2 = mjraw_SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2); int n2 = mjraw_SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found // return if two contacts already found
@@ -456,10 +458,10 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
} }
// x2 = 1 // x2 = 1
mju_add3(vec2, pos2, axis2); mji_add3(vec2, pos2, axis2);
mjtNum x1 = mju_clip((u - mb) / ma, -1, 1); mjtNum x1 = mju_clip((u - mb) / ma, -1, 1);
mju_scl3(vec1, axis1, x1); mji_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1); mji_addTo3(vec1, pos1);
int n3 = mjraw_SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2); int n3 = mjraw_SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found // return if two contacts already found
@@ -468,10 +470,10 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
} }
// x2 = -1 // x2 = -1
mju_sub3(vec2, pos2, axis2); mji_sub3(vec2, pos2, axis2);
x1 = mju_clip((u + mb) / ma, -1, 1); x1 = mju_clip((u + mb) / ma, -1, 1);
mju_scl3(vec1, axis1, x1); mji_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1); mji_addTo3(vec1, pos1);
int n4 = mjraw_SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2); int n4 = mjraw_SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
return n1+n2+n3+n4; return n1+n2+n3+n4;
@@ -533,7 +535,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
// N is normal to triangle plane // N is normal to triangle plane
mjtNum N[3]; mjtNum N[3];
mju_cross(N, A, B); mji_cross(N, A, B);
mju_normalize3(N); mju_normalize3(N);
// dstS is signed distance from S to plane; exit if too large // 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 // P is projection of S in triangle plane
mjtNum P[3]; mjtNum P[3];
mju_addScl3(P, S, N, -dstS); mji_addScl3(P, S, N, -dstS);
// construct orthogonal axes (V1~A, V2) of triangle plane // construct orthogonal axes (V1~A, V2) of triangle plane
mjtNum V1[3], V2[3]; mjtNum V1[3], V2[3];
mju_copy3(V1, A); mji_copy3(V1, A);
mjtNum lenA = mju_normalize3(V1); mjtNum lenA = mju_normalize3(V1);
mju_cross(V2, N, A); mji_cross(V2, N, A);
mju_normalize3(V2); mju_normalize3(V2);
// triangle is (o,a,b), sphere center is p // triangle is (o,a,b), sphere center is p
mjtNum o[2] = {0, 0}; 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 b[2] = {mju_dot3(V1, B), mju_dot3(V2, B)};
mjtNum p[2] = {mju_dot3(V1, P), mju_dot3(V2, P)}; 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 sign1 = areaSign(p, o, a);
mjtNum sign2 = areaSign(p, a, b); mjtNum sign2 = areaSign(p, a, b);
mjtNum sign3 = areaSign(p, b, o); mjtNum sign3 = areaSign(p, b, o);
@@ -567,7 +569,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
// p is inside triangle // p is inside triangle
if (sign1 == sign2 && sign2 == sign3) { if (sign1 == sign2 && sign2 == sign3) {
// P is nearest point to S within triangle // P is nearest point to S within triangle
mju_copy3(X, P); mji_copy3(X, P);
} }
// p is not inside triangle // 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); int best = (dstx[0] < dstx[1] && dstx[0] < dstx[2]) ? 0 : (dstx[1] < dstx[2] ? 1 : 2);
// convert x[best] to 3D // convert x[best] to 3D
mju_scl3(X, V1, x[best][0]); mji_scl3(X, V1, x[best][0]);
mju_addToScl3(X, V2, x[best][1]); mji_addToScl3(X, V2, x[best][1]);
} }
// X is now the nearest point to S within the 3D triangle (O,A,B) // 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 // construct contact
con[0].dist = dst - rs - rt; con[0].dist = dst - rs - rt;
mju_addScl3(con[0].pos, s, nrm, rs + con[0].dist/2); mji_addScl3(con[0].pos, s, nrm, rs + con[0].dist / 2);
mju_copy3(con[0].frame, nrm); mji_copy3(con[0].frame, nrm);
mju_zero3(con[0].frame+3); mju_zero3(con[0].frame+3);
return 1; 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_constraint.h"
#include "engine/engine_core_util.h" #include "engine/engine_core_util.h"
#include "engine/engine_crossplatform.h" #include "engine/engine_crossplatform.h"
#include "engine/engine_inline.h"
#include "engine/engine_macro.h" #include "engine/engine_macro.h"
#include "engine/engine_memory.h" #include "engine/engine_memory.h"
#include "engine/engine_sleep.h" #include "engine/engine_sleep.h"
@@ -67,13 +68,13 @@ void mj_kinematics1(const mjModel* m, mjData* d) {
int qadr = m->jnt_qposadr[jntadr]; int qadr = m->jnt_qposadr[jntadr];
// copy pos and quat from qpos // copy pos and quat from qpos
mju_copy3(xpos, d->qpos+qadr); mji_copy3(xpos, d->qpos+qadr);
mju_copy4(xquat, d->qpos+qadr+3); mji_copy4(xquat, d->qpos+qadr+3);
mju_normalize4(xquat); mju_normalize4(xquat);
// assign xanchor and xaxis // assign xanchor and xaxis
mju_copy3(d->xanchor+3*jntadr, xpos); mji_copy3(d->xanchor+3*jntadr, xpos);
mju_copy3(d->xaxis+3*jntadr, m->jnt_axis+3*jntadr); mji_copy3(d->xaxis+3*jntadr, m->jnt_axis+3*jntadr);
} }
// regular or no joint // regular or no joint
@@ -84,7 +85,7 @@ void mj_kinematics1(const mjModel* m, mjData* d) {
mjtNum *bodypos, *bodyquat, quat[4]; mjtNum *bodypos, *bodyquat, quat[4];
if (m->body_mocapid[i] >= 0) { if (m->body_mocapid[i] >= 0) {
bodypos = d->mocap_pos + 3*m->body_mocapid[i]; 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); mju_normalize4(quat);
bodyquat = quat; bodyquat = quat;
} else { } else {
@@ -94,13 +95,13 @@ void mj_kinematics1(const mjModel* m, mjData* d) {
// apply fixed translation and rotation relative to parent // apply fixed translation and rotation relative to parent
if (pid) { if (pid) {
mju_mulMatVec3(xpos, d->xmat+9*pid, bodypos); mji_mulMatVec3(xpos, d->xmat+9*pid, bodypos);
mju_addTo3(xpos, d->xpos+3*pid); mji_addTo3(xpos, d->xpos+3*pid);
mju_mulQuat(xquat, d->xquat+4*pid, bodyquat); mji_mulQuat(xquat, d->xquat+4*pid, bodyquat);
} else { } else {
// parent is the world // parent is the world
mju_copy3(xpos, bodypos); mji_copy3(xpos, bodypos);
mju_copy4(xquat, bodyquat); mji_copy4(xquat, bodyquat);
} }
// accumulate joints, compute xpos and xquat for this body // 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]; mjtJoint jtype = m->jnt_type[jid];
// compute axis in global frame; ball jnt_axis is (0,0,1), set by compiler // 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 // compute anchor in global frame
mju_rotVecQuat(xanchor, m->jnt_pos+3*jid, xquat); mji_rotVecQuat(xanchor, m->jnt_pos+3*jid, xquat);
mju_addTo3(xanchor, xpos); mji_addTo3(xanchor, xpos);
// apply joint transformation // apply joint transformation
switch (jtype) { switch (jtype) {
case mjJNT_SLIDE: case mjJNT_SLIDE:
mju_addToScl3(xpos, xaxis, d->qpos[qadr] - m->qpos0[qadr]); mji_addToScl3(xpos, xaxis, d->qpos[qadr] - m->qpos0[qadr]);
break; break;
case mjJNT_BALL: case mjJNT_BALL:
@@ -130,10 +131,10 @@ void mj_kinematics1(const mjModel* m, mjData* d) {
// compute local quaternion rotation // compute local quaternion rotation
mjtNum qloc[4]; mjtNum qloc[4];
if (jtype == mjJNT_BALL) { if (jtype == mjJNT_BALL) {
mju_copy4(qloc, d->qpos+qadr); mji_copy4(qloc, d->qpos+qadr);
mju_normalize4(qloc); mju_normalize4(qloc);
} else { } 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 // apply rotation
@@ -141,8 +142,8 @@ void mj_kinematics1(const mjModel* m, mjData* d) {
// correct for off-center rotation // correct for off-center rotation
mjtNum vec[3]; mjtNum vec[3];
mju_rotVecQuat(vec, m->jnt_pos+3*jid, xquat); mji_rotVecQuat(vec, m->jnt_pos+3*jid, xquat);
mju_sub3(xpos, xanchor, vec); mji_sub3(xpos, xanchor, vec);
} }
break; break;
@@ -151,8 +152,8 @@ void mj_kinematics1(const mjModel* m, mjData* d) {
} }
// assign xanchor and xaxis // assign xanchor and xaxis
mju_copy3(d->xanchor+3*jid, xanchor); mji_copy3(d->xanchor+3*jid, xanchor);
mju_copy3(d->xaxis+3*jid, xaxis); 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 // assign xquat and xpos, construct xmat
mju_copy4(d->xquat+4*i, xquat); mji_copy4(d->xquat+4*i, xquat);
mju_copy3(d->xpos+3*i, xpos); mji_copy3(d->xpos+3*i, xpos);
mju_quat2Mat(d->xmat+9*i, xquat); 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++) { for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : 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 // 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]; int parent = m->body_parentid[i];
if (sleep_filter && d->body_awake[i] == mjS_ASLEEP) { if (sleep_filter && d->body_awake[i] == mjS_ASLEEP) {
mjtNum child_moment[3]; mjtNum child_moment[3];
mju_scl3(child_moment, d->subtree_com+3*i, m->body_subtreemass[i]); mji_scl3(child_moment, d->subtree_com+3*i, m->body_subtreemass[i]);
mju_addTo3(d->subtree_com+3*parent, child_moment); mji_addTo3(d->subtree_com+3*parent, child_moment);
} else { } 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; int i = sleep_filter ? d->body_awake_ind[b] : b;
if (m->body_subtreemass[i] < mjMINVAL) { 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 { } 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; int i = sleep_filter ? d->body_awake_ind[b] : b;
mjtNum offset[3]; 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]); 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 // compute com-anchor vector
mjtNum offset[3], axis[3]; 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 // create motion dof
int skip = 0; int skip = 0;
@@ -377,16 +379,16 @@ void mj_camlight(const mjModel* m, mjData* d) {
case mjCAMLIGHT_TRACK: case mjCAMLIGHT_TRACK:
case mjCAMLIGHT_TRACKCOM: case mjCAMLIGHT_TRACKCOM:
// fixed global orientation // 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 // position: track camera body
if (m->cam_mode[i] == mjCAMLIGHT_TRACK) { if (m->cam_mode[i] == mjCAMLIGHT_TRACK) {
mju_add3(d->cam_xpos+3*i, d->xpos+3*id, m->cam_pos0+3*i); mji_add3(d->cam_xpos+3*i, d->xpos+3*id, m->cam_pos0+3*i);
} }
// position: track subtree com // position: track subtree com
else { 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; break;
@@ -397,25 +399,25 @@ void mj_camlight(const mjModel* m, mjData* d) {
mjtNum pos[3]; mjtNum pos[3];
// get position to look at // get position to look at
if (m->cam_mode[i] == mjCAMLIGHT_TARGETBODY) { if (m->cam_mode[i] == mjCAMLIGHT_TARGETBODY) {
mju_copy3(pos, d->xpos+3*id1); mji_copy3(pos, d->xpos+3*id1);
} else { } else {
mju_copy3(pos, d->subtree_com+3*id1); mji_copy3(pos, d->subtree_com+3*id1);
} }
// zaxis = -desired camera direction, in global frame // zaxis = -desired camera direction, in global frame
mjtNum matT[9]; 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); mju_normalize3(matT+6);
// xaxis: orthogonal to zaxis and to (0,0,1) // xaxis: orthogonal to zaxis and to (0,0,1)
matT[3] = 0; matT[3] = 0;
matT[4] = 0; matT[4] = 0;
matT[5] = 1; matT[5] = 1;
mju_cross(matT, matT+3, matT+6); mji_cross(matT, matT+3, matT+6);
mju_normalize3(matT); mju_normalize3(matT);
// yaxis: orthogonal to xaxis and zaxis // yaxis: orthogonal to xaxis and zaxis
mju_cross(matT+3, matT+6, matT); mji_cross(matT+3, matT+6, matT);
mju_normalize3(matT+3); mju_normalize3(matT+3);
// set camera frame // set camera frame
@@ -439,7 +441,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
// default processing for fixed mode // default processing for fixed mode
mj_local2Global(d, d->light_xpos+3*i, 0, m->light_pos+3*i, 0, id, 0); 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 // adjust for mode
switch ((mjtCamLight) m->light_mode[i]) { switch ((mjtCamLight) m->light_mode[i]) {
@@ -448,16 +450,16 @@ void mj_camlight(const mjModel* m, mjData* d) {
case mjCAMLIGHT_TRACK: case mjCAMLIGHT_TRACK:
case mjCAMLIGHT_TRACKCOM: case mjCAMLIGHT_TRACKCOM:
// fixed global orientation // 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 // position: track light body
if (m->light_mode[i] == mjCAMLIGHT_TRACK) { if (m->light_mode[i] == mjCAMLIGHT_TRACK) {
mju_add3(d->light_xpos+3*i, d->xpos+3*id, m->light_pos0+3*i); mji_add3(d->light_xpos+3*i, d->xpos+3*id, m->light_pos0+3*i);
} }
// position: track subtree com // position: track subtree com
else { 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; break;
@@ -468,13 +470,13 @@ void mj_camlight(const mjModel* m, mjData* d) {
// get position to look at // get position to look at
mjtNum lookat[3]; mjtNum lookat[3];
if (m->light_mode[i] == mjCAMLIGHT_TARGETBODY) { if (m->light_mode[i] == mjCAMLIGHT_TARGETBODY) {
mju_copy3(lookat, d->xpos+3*id1); mji_copy3(lookat, d->xpos+3*id1);
} else { } else {
mju_copy3(lookat, d->subtree_com+3*id1); mji_copy3(lookat, d->subtree_com+3*id1);
} }
// set dir // 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 // centered: copy body position
if (m->flex_centered[f]) { if (m->flex_centered[f]) {
for (int i=vstart; i < vend; i++) { 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 // non-centered: map from local to global
else { else {
for (int i=vstart; i < vend; i++) { 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); mji_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_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]; mjtNum nodexpos[3*mjMAXFLEXNODES];
if (m->flex_centered[f]) { if (m->flex_centered[f]) {
for (int i=nstart; i < nend; i++) { 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 { } else {
for (int i=nstart; i < nend; i++) { for (int i=nstart; i < nend; i++) {
int j = i - nstart; int j = i - nstart;
mju_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i); mji_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_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 // compute min and max along each global axis
mjtNum xmin[3], xmax[3]; mjtNum xmin[3], xmax[3];
mju_copy3(xmin, vert+3*edata[0]); mji_copy3(xmin, vert+3*edata[0]);
mju_copy3(xmax, vert+3*edata[0]); mji_copy3(xmax, vert+3*edata[0]);
for (int i=1; i <= dim; i++) { for (int i=1; i <= dim; i++) {
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
mjtNum value = vert[3*edata[i]+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 // copy element aabbs to bhv leaf aabbs
for (int i=flex_bvhadr; i < flex_bvhadr+flex_bvhnum; i++) { for (int i=flex_bvhadr; i < flex_bvhadr+flex_bvhnum; i++) {
if (m->bvh_nodeid[i] >= 0) { if (m->bvh_nodeid[i] >= 0) {
mju_copy(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic), mji_copy6(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic),
d->flexelem_aabb + 6*(m->flex_elemadr[f] + m->bvh_nodeid[i]), 6); 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 // vec = unit vector from v1 to v2, compute edge length
mjtNum vec[3]; mjtNum vec[3];
mju_sub3(vec, pos2, pos1); mji_sub3(vec, pos2, pos1);
d->flexedge_length[ebase+e] = mju_normalize3(vec); d->flexedge_length[ebase+e] = mju_normalize3(vec);
// skip Jacobian if not needed // skip Jacobian if not needed
@@ -851,7 +853,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
mjtNum wlen = -1; mjtNum wlen = -1;
int wrapid = -1; int wrapid = -1;
mjtNum wpnt[12]; mjtNum wpnt[12];
mju_copy3(wpnt, d->site_xpos+3*id0); mji_copy3(wpnt, d->site_xpos+3*id0);
int wbody[4]; int wbody[4];
wbody[0] = m->site_bodyid[id0]; wbody[0] = m->site_bodyid[id0];
@@ -878,11 +880,11 @@ void mj_tendon(const mjModel* m, mjData* d) {
// complete sequence, accumulate lengths // complete sequence, accumulate lengths
if (wlen < 0) { 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]; wbody[1] = m->site_bodyid[id1];
L[i] += mju_dist3(wpnt, wpnt+3) / divisor; L[i] += mju_dist3(wpnt, wpnt+3) / divisor;
} else { } 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[1] = wbody[2] = m->geom_bodyid[wrapid];
wbody[3] = m->site_bodyid[id1]; wbody[3] = m->site_bodyid[id1];
L[i] += (mju_dist3(wpnt, wpnt+3) + wlen + mju_dist3(wpnt+6, wpnt+9)) / divisor; 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]) { if (wbody[k] != wbody[k+1]) {
// get 3D position difference, normalize // get 3D position difference, normalize
mjtNum dif[3]; 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); mju_normalize3(dif);
// sparse // sparse
@@ -934,7 +936,11 @@ void mj_tendon(const mjModel* m, mjData* d) {
} }
// assign to wrap // 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; d->wrap_obj[wrapcount] = -1;
if (wlen >= 0) { if (wlen >= 0) {
d->wrap_obj[wrapcount+1] = d->wrap_obj[wrapcount+2] = wrapid; 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 // assign last site before pulley or tendon end
if (j == tendon_num-1 || m->wrap_type[adr+j+1] == mjWRAP_PULLEY) { 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->wrap_obj[wrapcount] = -1;
d->ten_wrapnum[i]++; d->ten_wrapnum[i]++;
wrapcount++; wrapcount++;
@@ -1009,7 +1015,7 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) {
// init sequence; assume it starts with site // init sequence; assume it starts with site
mjtNum wpnt[6]; mjtNum wpnt[6];
mju_copy3(wpnt, d->site_xpos+3*id0); mji_copy3(wpnt, d->site_xpos+3*id0);
mjtNum vel[6]; mjtNum vel[6];
mj_objectVelocity(m, d, mjOBJ_SITE, id0, vel, /*flg_local=*/0); mj_objectVelocity(m, d, mjOBJ_SITE, id0, vel, /*flg_local=*/0);
mjtNum wvel[6] = {vel[3], vel[4], vel[5], 0, 0, 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 // complete sequence
wbody[1] = m->site_bodyid[id1]; 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); 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 // accumulate moments if consecutive points are in different bodies
if (wbody[0] != wbody[1]) { 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) { else if (m->jnt_type[id] == mjJNT_BALL) {
// axis: expmap representation of quaternion // axis: expmap representation of quaternion
mjtNum axis[3], quat[4]; 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_normalize4(quat);
mju_quat2Vel(axis, quat, 1); mji_quat2Vel(axis, quat, 1);
// gearAxis: rotate to parent frame if necessary // gearAxis: rotate to parent frame if necessary
mjtNum gearAxis[3]; mjtNum gearAxis[3];
if (m->actuator_trntype[i] == mjTRN_JOINT) { if (m->actuator_trntype[i] == mjTRN_JOINT) {
mju_copy3(gearAxis, gear); mji_copy3(gearAxis, gear);
} else { } else {
mju_negQuat(quat, quat); mji_negQuat(quat, quat);
mju_rotVecQuat(gearAxis, gear, quat); mji_rotVecQuat(gearAxis, gear, quat);
} }
// length: axis*gearAxis // length: axis*gearAxis
@@ -1170,7 +1176,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
rownnz[i] = 3; rownnz[i] = 3;
// moment: gearAxis // moment: gearAxis
mju_copy3(moment+adr, gearAxis); mji_copy3(moment+adr, gearAxis);
} }
// free joint: 6D wrench gear // free joint: 6D wrench gear
@@ -1181,13 +1187,13 @@ void mj_transmission(const mjModel* m, mjData* d) {
// gearAxis: rotate to world frame if necessary // gearAxis: rotate to world frame if necessary
mjtNum gearAxis[3]; mjtNum gearAxis[3];
if (m->actuator_trntype[i] == mjTRN_JOINT) { if (m->actuator_trntype[i] == mjTRN_JOINT) {
mju_copy3(gearAxis, gear+3); mji_copy3(gearAxis, gear+3);
} else { } else {
mjtNum quat[4]; 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_normalize4(quat);
mju_negQuat(quat, quat); mju_negQuat(quat, quat);
mju_rotVecQuat(gearAxis, gear+3, quat); mji_rotVecQuat(gearAxis, gear+3, quat);
} }
// dof start address // dof start address
@@ -1200,8 +1206,8 @@ void mj_transmission(const mjModel* m, mjData* d) {
rownnz[i] = 6; rownnz[i] = 6;
// moment: gear(tran), gearAxis // moment: gear(tran), gearAxis
mju_copy3(moment+adr, gear); mji_copy3(moment+adr, gear);
mju_copy3(moment+adr+3, gearAxis); mji_copy3(moment+adr+3, gearAxis);
} }
break; break;
@@ -1235,12 +1241,12 @@ void mj_transmission(const mjModel* m, mjData* d) {
if (ok) { if (ok) {
mju_scl3(dldv, axis, 1-av/sdet); mju_scl3(dldv, axis, 1-av/sdet);
mju_scl3(dlda, vec, 1/sdet); // use dlda as temp 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); mju_scl3(dlda, vec, 1-av/sdet);
} else { } else {
mju_copy3(dlda, vec); mji_copy3(dlda, vec);
mju_copy3(dldv, axis); mji_copy3(dldv, axis);
} }
// get Jacobians of axis(jacA) and vec(jac) // 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) { if (m->actuator_trnid[2*i+1] == -1) {
// wrench: gear expressed in global frame // wrench: gear expressed in global frame
mjtNum wrench[6]; mjtNum wrench[6];
mju_mulMatVec3(wrench, d->site_xmat+9*id, gear); // translation mji_mulMatVec3(wrench, d->site_xmat+9*id, gear); // translation
mju_mulMatVec3(wrench+3, d->site_xmat+9*id, gear+3); // rotation mji_mulMatVec3(wrench+3, d->site_xmat+9*id, gear+3); // rotation
// moment: global Jacobian projected on wrench // moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+adr, jac, wrench, 3, nv); // translation 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 // wrench: translational gear expressed in global frame
mjtNum wrench[6]; 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 // moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+adr, jac, wrench, 3, nv); 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) // get site and refsite quats from parent bodies (avoiding mju_mat2Quat)
mjtNum quat[4]; mjtNum quat[4];
mju_mulQuat(quat, m->site_quat+4*id, d->xquat+4*m->site_bodyid[id]); mji_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(refquat, m->site_quat+4*refid, d->xquat+4*m->site_bodyid[refid]);
// convert difference to expmap (axis-angle) // convert difference to expmap (axis-angle)
mjtNum vec[3]; mjtNum vec[3];
mju_subQuat(vec, quat, refquat); mji_subQuat(vec, quat, refquat);
// add length: dot product with gear // add length: dot product with gear
length[i] += mju_dot3(vec, gear+3); 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 // wrench: rotational gear expressed in global frame
mjtNum wrench[6]; 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 // moment_tmp: global Jacobian projected on wrench, add to moment
if (!moment_tmp) moment_tmp = mjSTACKALLOC(d, nv, mjtNum); 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 // sparse backward pass over ancestors
for (int j=i; j >= 0; j = dof_parentid[j]) { for (int j=i; j >= 0; j = dof_parentid[j]) {
// M(i,j) += cdof_j * (crb_body_i * cdof_i) // 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 // cvel = cvel_parent
mjtNum cvel[6]; 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 // cvel = cvel_parent + cdof * qvel, cdofdot = cvel x cdof
int dofnum = m->body_dofnum[i]; int dofnum = m->body_dofnum[i];
@@ -2144,9 +2150,9 @@ void mj_comVel(const mjModel* m, mjData* d) {
case mjJNT_BALL: case mjJNT_BALL:
// compute all 3 cdofdots using parent velocity // compute all 3 cdofdots using parent velocity
mju_crossMotion(cdofdot+6*(j+0), cvel, d->cdof+6*(bda+j+0)); mji_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)); mji_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+2), cvel, d->cdof+6*(bda+j+2));
// update velocity // update velocity
mju_mulDofVec(tmp, d->cdof+6*(bda+j), d->qvel+bda+j, 3); 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, // in principle we should use the new velocity to compute cdofdot,
// but it makes no difference because crossMotion(cdof, cdof) = 0, // but it makes no difference because crossMotion(cdof, cdof) = 0,
// and using the old velocity may be more accurate numerically // 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 // update velocity
mju_mulDofVec(tmp, d->cdof+6*(bda+j), d->qvel+bda+j, 1); 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 // 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); 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[0] *= m->body_inertia[3*i];
dv[1] *= m->body_inertia[3*i+1]; dv[1] *= m->body_inertia[3*i+1];
dv[2] *= m->body_inertia[3*i+2]; 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 // subtree linear velocity
@@ -2208,7 +2214,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
// non-world: add linear momentum to parent // non-world: add linear momentum to parent
if (i) { 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 // 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(dx, d->xipos+3*i, d->subtree_com+3*i);
mju_sub3(dv, body_vel+6*i+3, d->subtree_linvel+3*i); mju_sub3(dv, body_vel+6*i+3, d->subtree_linvel+3*i);
mju_scl3(dp, dv, m->body_mass[i]); mju_scl3(dp, dv, m->body_mass[i]);
mju_cross(dL, dx, dp); mji_cross(dL, dx, dp);
// add to subtree i // add to subtree i
mju_addTo3(d->subtree_angmom+3*i, dL); mji_addTo3(d->subtree_angmom+3*i, dL);
// add to parent // 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 // momentum wrt parent
mju_sub3(dx, d->subtree_com+3*i, d->subtree_com+3*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_sub3(dv, d->subtree_linvel+3*i, d->subtree_linvel+3*parent);
mju_scl3(dv, dv, m->body_subtreemass[i]); mju_scl3(dv, dv, m->body_subtreemass[i]);
mju_cross(dL, dx, dv); mji_cross(dL, dx, dv);
// add to parent // add to parent
mju_addTo3(d->subtree_angmom+3*parent, dL); mji_addTo3(d->subtree_angmom+3*parent, dL);
} }
mj_freeStack(d); 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(loc_cfrc_body+6*i, d->cinert+10*i, loc_cacc+6*i);
mju_mulInertVec(tmp, d->cinert+10*i, d->cvel+6*i); mju_mulInertVec(tmp, d->cinert+10*i, d->cvel+6*i);
mjtNum tmp1[6]; 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); 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 // result = cdof * cfrc_body
for (int v=0; v < nv; v++) { for (int v=0; v < nv; v++) {
int i = sleep_filter ? d->dof_awake_ind[v] : 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); mj_freeStack(d);
@@ -2334,8 +2340,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
for (int i=1; i < nbody; i++) { for (int i=1; i < nbody; i++) {
if (!mju_isZero(d->xfrc_applied+6*i, 6)) { if (!mju_isZero(d->xfrc_applied+6*i, 6)) {
// rearrange as torque:force // rearrange as torque:force
mju_copy3(cfrc, d->xfrc_applied+6*i+3); mji_copy3(cfrc, d->xfrc_applied+6*i+3);
mju_copy3(cfrc+3, d->xfrc_applied+6*i); mji_copy3(cfrc+3, d->xfrc_applied+6*i);
// map force from application point to com; both world-oriented // 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); 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_CONNECT:
case mjEQ_WELD: case mjEQ_WELD:
// cfrc = world-oriented torque:force vector // 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) { if (m->eq_type[id] == mjEQ_WELD) {
mju_copy3(cfrc, d->efc_force + i + 3); mji_copy3(cfrc, d->efc_force + i + 3);
} else { } else {
mju_zero3(cfrc); // no torque from connect 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) // cfrc_body = cinert * cacc + cvel x (cinert * cvel)
mju_mulInertVec(cfrc_body, d->cinert+10*j, d->cacc+6*j); 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_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); mju_addTo(cfrc_body, cfrc, 6);
// set cfrc_int = cfrc_body - cfrc_ext // 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/mjdata.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include "engine/engine_inline.h"
#include "engine/engine_memory.h" #include "engine/engine_memory.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.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) // construct translation jacobian (correct for rotation)
if (jacp) { if (jacp) {
mjtNum tmp[3]; mjtNum tmp[3];
mju_cross(tmp, cdof, offset); mji_cross(tmp, cdof, offset);
jacp[i+0*nv] = cdof[3] + tmp[0]; jacp[i+0*nv] = cdof[3] + tmp[0];
jacp[i+1*nv] = cdof[4] + tmp[1]; jacp[i+1*nv] = cdof[4] + tmp[1];
jacp[i+2*nv] = cdof[5] + tmp[2]; 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) // construct translation jacobian (correct for rotation)
if (jacp) { if (jacp) {
mjtNum tmp[3]; mjtNum tmp[3];
mju_cross(tmp, cdof, offset); mji_cross(tmp, cdof, offset);
jacp[ci+0*NV] = cdof[3] + tmp[0]; jacp[ci+0*NV] = cdof[3] + tmp[0];
jacp[ci+1*NV] = cdof[4] + tmp[1]; 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) // construct translation jacobian (correct for rotation)
if (jacdifp) { if (jacdifp) {
mjtNum tmp[3]; mjtNum tmp[3];
mju_cross(tmp, cdof, offset); mji_cross(tmp, cdof, offset);
// plus sign // plus sign
if (flg_second) { if (flg_second) {
@@ -616,7 +617,7 @@ void mj_jacDot(const mjModel* m, const mjData* d,
// backward pass over dof ancestor chain // backward pass over dof ancestor chain
while (i >= 0) { while (i >= 0) {
mjtNum cdof_dot[6]; 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; mjtNum* cdof = d->cdof+6*i;
// check for quaternion // 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) // compute cdof_dot for quaternion (use current body cvel)
if (is_quat) { 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 // construct rotation jacobian
@@ -640,11 +641,11 @@ void mj_jacDot(const mjModel* m, const mjData* d,
if (jacp) { if (jacp) {
// first correction term, account for varying cdof // first correction term, account for varying cdof
mjtNum tmp1[3]; mjtNum tmp1[3];
mju_cross(tmp1, cdof_dot, offset); mji_cross(tmp1, cdof_dot, offset);
// second correction term, account for point translational velocity // second correction term, account for point translational velocity
mjtNum tmp2[3]; 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+0*nv] += cdof_dot[3] + tmp1[0] + tmp2[0];
jacp[i+1*nv] += cdof_dot[4] + tmp1[1] + tmp2[1]; 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 // orientation of the COM (inertial) frame of b-th body
mjtNum ximat[9]; 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 // save the inertia matrix of b-th body
mjtNum inertia[9] = {0}; 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 // term1 = body angular momentum about self COM in world frame
mjtNum tmp1[9], tmp2[9]; 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_mulMatMatT3(tmp2, tmp1, ximat); // tmp2 = ximat * inertia * ximat^T
mju_mulMatMat(term1, tmp2, jacr, 3, 3, nv); // term1 = ximat * inertia * ximat^T * jacr mju_mulMatMat(term1, tmp2, jacr, 3, 3, nv); // term1 = ximat * inertia * ximat^T * jacr
// location of body COM w.r.t subtree COM // location of body COM w.r.t subtree COM
mjtNum com[3]; 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 // skew symmetric matrix representing body_com vector
mjtNum com_mat[9] = {0}; 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 // add Coriolis correction due to rotating frame: acc_tran += vel_rot x vel_tran
mjtNum correction[3]; mjtNum correction[3];
mju_cross(correction, vel, vel+3); mji_cross(correction, vel, vel+3);
mju_addTo3(res+3, correction); 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_NONE:
case mjSAMEFRAME_BODYROT: case mjSAMEFRAME_BODYROT:
case mjSAMEFRAME_INERTIAROT: case mjSAMEFRAME_INERTIAROT:
mju_mulMatVec3(xpos, d->xmat+9*body, pos); mji_mulMatVec3(xpos, d->xmat+9*body, pos);
mju_addTo3(xpos, d->xpos+3*body); mji_addTo3(xpos, d->xpos+3*body);
break; break;
case mjSAMEFRAME_BODY: case mjSAMEFRAME_BODY:
mju_copy3(xpos, d->xpos+3*body); mji_copy3(xpos, d->xpos+3*body);
break; break;
case mjSAMEFRAME_INERTIA: case mjSAMEFRAME_INERTIA:
mju_copy3(xpos, d->xipos+3*body); mji_copy3(xpos, d->xipos+3*body);
break; break;
} }
} }
@@ -880,16 +881,16 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
mjtNum tmp[4]; mjtNum tmp[4];
switch (sf) { switch (sf) {
case mjSAMEFRAME_NONE: case mjSAMEFRAME_NONE:
mju_mulQuat(tmp, d->xquat+4*body, quat); mji_mulQuat(tmp, d->xquat+4*body, quat);
mju_quat2Mat(xmat, tmp); mju_quat2Mat(xmat, tmp);
break; break;
case mjSAMEFRAME_BODY: case mjSAMEFRAME_BODY:
case mjSAMEFRAME_BODYROT: case mjSAMEFRAME_BODYROT:
mju_copy9(xmat, d->xmat+9*body); mji_copy9(xmat, d->xmat+9*body);
break; break;
case mjSAMEFRAME_INERTIA: case mjSAMEFRAME_INERTIA:
case mjSAMEFRAME_INERTIAROT: case mjSAMEFRAME_INERTIAROT:
mju_copy9(xmat, d->ximat+9*body); mji_copy9(xmat, d->ximat+9*body);
break; break;
} }
} }
@@ -957,4 +958,3 @@ void mj_warning(mjData* d, int warning, int info) {
// increase counter // increase counter
d->warning[warning].number++; 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_constraint.h"
#include "engine/engine_core_util.h" #include "engine/engine_core_util.h"
#include "engine/engine_crossplatform.h" #include "engine/engine_crossplatform.h"
#include "engine/engine_inline.h"
#include "engine/engine_memory.h" #include "engine/engine_memory.h"
#include "engine/engine_plugin.h" #include "engine/engine_plugin.h"
#include "engine/engine_sleep.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" // convert quaternion difference into angular "velocity"
mjtNum dif[3], quat[4]; mjtNum dif[3], quat[4];
mju_copy4(quat, d->qpos+padr); mji_copy4(quat, d->qpos+padr);
mju_normalize4(quat); mju_normalize4(quat);
mju_subQuat(dif, quat, m->qpos_spring + padr); mji_subQuat(dif, quat, m->qpos_spring + padr);
// apply torque // apply torque
d->qfrc_spring[dadr+0] = -stiffness*dif[0]; d->qfrc_spring[dadr+0] = -stiffness*dif[0];
@@ -161,15 +162,15 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// flap edges // flap edges
mjtNum ed[3][3]; mjtNum ed[3][3];
mju_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]); mji_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]);
mju_sub3(ed[1], xpos + 3*v[2], xpos + 3*v[0]); mji_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[2], xpos + 3*v[3], xpos + 3*v[0]);
// forces at the vertices due to curved reference // forces at the vertices due to curved reference
mjtNum frc[4][3]; mjtNum frc[4][3];
mju_cross(frc[1], ed[1], ed[2]); mji_cross(frc[1], ed[1], ed[2]);
mju_cross(frc[2], ed[2], ed[0]); mji_cross(frc[2], ed[2], ed[0]);
mju_cross(frc[3], ed[0], ed[1]); mji_cross(frc[3], ed[0], ed[1]);
frc[0][0] = -(frc[1][0] + frc[2][0] + frc[3][0]); 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][1] = -(frc[1][1] + frc[2][1] + frc[3][1]);
frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]); 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 // compute positions
if (m->flex_centered[f]) { if (m->flex_centered[f]) {
for (int i=0; i < nodenum; i++) { for (int i=0; i < nodenum; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); mji_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]); mji_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]);
} }
} else { } else {
mjtNum screw[6]; mjtNum screw[6];
for (int i=0; i < nodenum; i++) { for (int i=0; i < nodenum; i++) {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart)); mji_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_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0); 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 // compute center of mass
for (int i = 0; i < nodenum; i++) { 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 // re-center positions using center of mass
for (int i = 0; i < nodenum; i++) { 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 // 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 // rotate vertices to quat and add reference center of mass
for (int i = 0; i < nodenum; i++) { for (int i = 0; i < nodenum; i++) {
mju_rotVecQuat(xpos+3*i, xpos+3*i, quat); 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); mju_rotVecQuat(vel+3*i, vel+3*i, quat);
} }
// compute displacement // compute displacement
for (int i = 0; i < nodenum; i++) { 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 // compute force in the stretch frame
@@ -281,12 +282,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mju_negQuat(quat, quat); mju_negQuat(quat, quat);
for (int i = 0; i < nodenum; i++) { for (int i = 0; i < nodenum; i++) {
mjtNum qfrc[3], qdmp[3]; mjtNum qfrc[3], qdmp[3];
mju_rotVecQuat(qfrc, frc+3*i, quat); mji_rotVecQuat(qfrc, frc+3*i, quat);
mju_rotVecQuat(qdmp, dmp+3*i, quat); mji_rotVecQuat(qdmp, dmp+3*i, quat);
mju_scl3(qdmp, qdmp, m->flex_damping[f]); mju_scl3(qdmp, qdmp, m->flex_damping[f]);
if (m->flex_centered[f]) { if (m->flex_centered[f]) {
if (has_spring) mju_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc); if (has_spring) mji_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_damping) mji_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp);
} else { } else {
if (has_spring) mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring); 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); 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; int i = sleep_filter ? d->body_awake_ind[b] : b;
if (m->body_gravcomp[i]) { if (m->body_gravcomp[i]) {
has_gravcomp = 1; 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); 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 // compute wind in local coordinates
mju_zero(wind, 6); 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, mju_transformSpatial(lwind, wind, 0, d->xipos+3*i,
d->subtree_com+3*m->body_rootid[i], d->ximat+9*i); d->subtree_com+3*m->body_rootid[i], d->ximat+9*i);
// subtract translational component from body velocity // subtract translational component from body velocity
mju_subFrom3(lvel+3, lwind+3); mji_subFrom3(lvel+3, lwind+3);
mju_zero(lfrc, 6); mju_zero(lfrc, 6);
// set viscous force and torque // 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; diam = (box[0] + box[1] + box[2])/3.0;
// angular viscosity // 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 // 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 // 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; mju_abs(lvel[2])*lvel[2]/64.0;
} }
// rotate to global orientation: lfrc -> bfrc // rotate to global orientation: lfrc -> bfrc
mju_mulMatVec3(bfrc, d->ximat+9*i, lfrc); mji_mulMatVec3(bfrc, d->ximat+9*i, lfrc);
mju_mulMatVec3(bfrc+3, d->ximat+9*i, lfrc+3); mji_mulMatVec3(bfrc+3, d->ximat+9*i, lfrc+3);
// apply force and torque to body com // apply force and torque to body com
mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_fluid); 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 // compute wind in local coordinates
mju_zero(wind, 6); mju_zero(wind, 6);
mju_copy3(wind+3, m->opt.wind); mji_copy3(wind+3, m->opt.wind);
mju_transformSpatial(lwind, wind, 0, mju_transformSpatial(lwind, wind, 0,
d->geom_xpos + 3*geomid, // Frame of ref's origin. d->geom_xpos + 3*geomid, // Frame of ref's origin.
d->subtree_com + 3*m->body_rootid[bodyid], d->subtree_com + 3*m->body_rootid[bodyid],
d->geom_xmat + 9*geomid); // Frame of ref's orientation. d->geom_xmat + 9*geomid); // Frame of ref's orientation.
// subtract translational component from grom velocity // subtract translational component from grom velocity
mju_subFrom3(lvel+3, lwind+3); mji_subFrom3(lvel+3, lwind+3);
// initialize viscous force and torque // initialize viscous force and torque
mju_zero(lfrc, 6); 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); mju_scl(lfrc, lfrc, geom_interaction_coef, 6);
// rotate to global orientation: lfrc -> bfrc // rotate to global orientation: lfrc -> bfrc
mju_mulMatVec3(bfrc, d->geom_xmat + 9*geomid, lfrc); mji_mulMatVec3(bfrc, d->geom_xmat + 9*geomid, lfrc);
mju_mulMatVec3(bfrc+3, d->geom_xmat + 9*geomid, lfrc+3); mji_mulMatVec3(bfrc+3, d->geom_xmat + 9*geomid, lfrc+3);
// apply force and torque to body com // apply force and torque to body com
mj_applyFT(m, d, bfrc+3, bfrc, 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]; mjtNum added_mass_force[3], added_mass_torque1[3], added_mass_torque2[3];
mju_cross(added_mass_force, virtual_lin_mom, ang_vel); mji_cross(added_mass_force, virtual_lin_mom, ang_vel);
mju_cross(added_mass_torque1, virtual_lin_mom, lin_vel); mji_cross(added_mass_torque1, virtual_lin_mom, lin_vel);
mju_cross(added_mass_torque2, virtual_ang_mom, ang_vel); mji_cross(added_mass_torque2, virtual_ang_mom, ang_vel);
mju_addTo3(local_force, added_mass_torque1); mji_addTo3(local_force, added_mass_torque1);
mju_addTo3(local_force, added_mass_torque2); mji_addTo3(local_force, added_mass_torque2);
mju_addTo3(local_force+3, added_mass_force); mji_addTo3(local_force+3, added_mass_force);
} }
@@ -926,7 +927,7 @@ void mj_viscousForces(
const mjtNum A_max = mjPI * d_max * d_mid; const mjtNum A_max = mjPI * d_max * d_mid;
mjtNum magnus_force[3]; 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[0] *= magnus_lift_coef * fluid_density * volume;
magnus_force[1] *= magnus_lift_coef * fluid_density * volume; magnus_force[1] *= magnus_lift_coef * fluid_density * volume;
magnus_force[2] *= 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( const mjtNum cos_alpha = proj_num / mju_max(
mjMINVAL, mju_norm3(lin_vel) * proj_denom); mjMINVAL, mju_norm3(lin_vel) * proj_denom);
mjtNum kutta_circ[3]; 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[0] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
kutta_circ[1] *= 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; kutta_circ[2] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
mjtNum kutta_force[3]; 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 // 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]); 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_core_util.h"
#include "engine/engine_crossplatform.h" #include "engine/engine_crossplatform.h"
#include "engine/engine_memory.h" #include "engine/engine_memory.h"
#include "engine/engine_memory.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.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/mjdata.h>
#include <mujoco/mjmacro.h> #include <mujoco/mjmacro.h>
#include <mujoco/mjsan.h> // IWYU pragma: keep #include <mujoco/mjsan.h> // IWYU pragma: keep
#include "engine/engine_inline.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.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++) { for (iter=0; iter < 500; iter++) {
// make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec // make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec
mju_quat2Mat(eigvec, quat); mju_quat2Mat(eigvec, quat);
mju_mulMatTMat3(tmp, eigvec, mat); mji_mulMatTMat3(tmp, eigvec, mat);
mju_mulMatMat3(D, tmp, eigvec); mji_mulMatMat3(D, tmp, eigvec);
// assign eigenvalues // assign eigenvalues
eigval[0] = D[0]; eigval[0] = D[0];
src/engine/engine_util_spatial.c
+50 -49
View File
@@ -17,6 +17,7 @@
#include <math.h> #include <math.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include "engine/engine_inline.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.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 // null quat: copy vec
else if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) { else if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
mju_copy3(res, vec); mji_copy3(res, vec);
} }
// regular processing // regular processing
@@ -124,7 +125,7 @@ void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) {
} }
speed /= 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]) { void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) {
// qdif = neg(qb)*qa // qdif = neg(qb)*qa
mjtNum qneg[4], qdif[4]; mjtNum qneg[4], qdif[4];
mju_negQuat(qneg, qb); mji_negQuat(qneg, qb);
mju_mulQuat(qdif, qneg, qa); mji_mulQuat(qdif, qneg, qa);
// convert to 3D velocity // 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 // regular processing
else { else {
const mjtNum q00 = quat[0]*quat[0]; mjtNum q00 = quat[0]*quat[0];
const mjtNum q01 = quat[0]*quat[1]; mjtNum q01 = quat[0]*quat[1];
const mjtNum q02 = quat[0]*quat[2]; mjtNum q02 = quat[0]*quat[2];
const mjtNum q03 = quat[0]*quat[3]; mjtNum q03 = quat[0]*quat[3];
const mjtNum q11 = quat[1]*quat[1]; mjtNum q11 = quat[1]*quat[1];
const mjtNum q12 = quat[1]*quat[2]; mjtNum q12 = quat[1]*quat[2];
const mjtNum q13 = quat[1]*quat[3]; mjtNum q13 = quat[1]*quat[3];
const mjtNum q22 = quat[2]*quat[2]; mjtNum q22 = quat[2]*quat[2];
const mjtNum q23 = quat[2]*quat[3]; mjtNum q23 = quat[2]*quat[3];
const mjtNum q33 = quat[3]*quat[3]; mjtNum q33 = quat[3]*quat[3];
res[0] = q00 + q11 - q22 - q33; res[0] = q00 + q11 - q22 - q33;
res[4] = 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]; mjtNum angle, tmp[4], qrot[4];
// form local rotation quaternion, apply // form local rotation quaternion, apply
mju_copy3(tmp, vel); mji_copy3(tmp, vel);
angle = scale * mju_normalize3(tmp); angle = scale * mju_normalize3(tmp);
mju_axisAngle2Quat(qrot, tmp, angle); mji_axisAngle2Quat(qrot, tmp, angle);
mju_normalize4(quat); mju_normalize4(quat);
mju_mulQuat(quat, quat, qrot); mju_mulQuat(quat, quat, qrot);
} }
@@ -256,7 +257,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
} }
// compute angle and axis // compute angle and axis
mju_cross(axis, z, vn); mji_cross(axis, z, vn);
a = mju_normalize3(axis); a = mju_normalize3(axis);
// almost parallel // almost parallel
@@ -272,7 +273,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
// make quaternion from angle and axis // make quaternion from angle and axis
a = mju_atan2(a, mju_dot3(vn, z)); 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 col2_rot[3] = {rot[1], rot[4], rot[7]};
mjtNum col3_rot[3] = {rot[2], rot[5], rot[8]}; mjtNum col3_rot[3] = {rot[2], rot[5], rot[8]};
mjtNum omega[3], vec1[3], vec2[3], vec3[3]; mjtNum omega[3], vec1[3], vec2[3], vec3[3];
mju_cross(vec1, col1_rot, col1_mat); mji_cross(vec1, col1_rot, col1_mat);
mju_cross(vec2, col2_rot, col2_mat); mji_cross(vec2, col2_rot, col2_mat);
mju_cross(vec3, col3_rot, col3_mat); mji_cross(vec3, col3_rot, col3_mat);
mju_add3(omega, vec1, vec2); mji_add3(omega, vec1, vec2);
mju_addTo3(omega, vec3); mji_addTo3(omega, vec3);
mju_scl3(omega, omega, 1.0 / (mju_abs(mju_dot3(col1_rot, col1_mat) + mju_scl3(omega, omega, 1.0 / (mju_abs(mju_dot3(col1_rot, col1_mat) +
mju_dot3(col2_rot, col2_mat) + mju_dot3(col2_rot, col2_mat) +
mju_dot3(col3_rot, col3_mat)) + mjMINVAL)); mju_dot3(col3_rot, col3_mat)) + mjMINVAL));
@@ -308,7 +309,7 @@ int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) {
break; break;
} }
mjtNum qrot[4]; mjtNum qrot[4];
mju_axisAngle2Quat(qrot, omega, w); mji_axisAngle2Quat(qrot, omega, w);
mju_mulQuat(quat, qrot, quat); mju_mulQuat(quat, qrot, quat);
mju_normalize4(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 pos1[3], const mjtNum quat1[4],
const mjtNum pos2[3], const mjtNum quat2[4]) { const mjtNum pos2[3], const mjtNum quat2[4]) {
// quatres = quat1*quat2 // quatres = quat1*quat2
mju_mulQuat(quatres, quat1, quat2); mji_mulQuat(quatres, quat1, quat2);
mju_normalize4(quatres); mju_normalize4(quatres);
// posres = quat1*pos2 + pos1 // posres = quat1*pos2 + pos1
mju_rotVecQuat(posres, pos2, quat1); mji_rotVecQuat(posres, pos2, quat1);
mju_addTo3(posres, pos1); mji_addTo3(posres, pos1);
} }
// negate pose // negate pose
void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const mjtNum quat[4]) { void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const mjtNum quat[4]) {
// qres = neg(quat) // qres = neg(quat)
mju_negQuat(quatres, quat); mji_negQuat(quatres, quat);
// pres = -neg(quat)*pos // pres = -neg(quat)*pos
mju_rotVecQuat(posres, pos, quatres); mji_rotVecQuat(posres, pos, quatres);
mju_scl3(posres, posres, -1); 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 // transform vector by pose
void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], const mjtNum vec[3]) { void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], const mjtNum vec[3]) {
// res = quat*vec + pos // res = quat*vec + pos
mju_rotVecQuat(res, vec, quat); mji_rotVecQuat(res, vec, quat);
mju_addTo3(res, pos); 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 // 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) { const mjtNum dif[3], mjtNum mass) {
// tmp = diag(inert) * mat' (mat is local-to-global rotation) // 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], 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 // 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 // hinge
if (offset) { if (offset) {
mju_copy3(res, axis); mji_copy3(res, axis);
mju_cross(res+3, axis, offset); mji_cross(res+3, axis, offset);
} }
// slide // slide
else { else {
mju_zero3(res); 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 // apply translation
mju_copy(tran, vec, 6); mju_copy(tran, vec, 6);
mju_sub3(dif, newpos, oldpos); mji_sub3(dif, newpos, oldpos);
if (flg_force) { if (flg_force) {
mju_cross(cros, dif, vec+3); mji_cross(cros, dif, vec+3);
mju_sub3(tran, vec, cros); mji_sub3(tran, vec, cros);
} else { } else {
mju_cross(cros, dif, vec); mji_cross(cros, dif, vec);
mju_sub3(tran+3, vec+3, cros); mji_sub3(tran+3, vec+3, cros);
} }
// if provided, apply old -> new rotation // if provided, apply old -> new rotation
if (rotnew2old) { if (rotnew2old) {
mju_mulMatTVec3(res, rotnew2old, tran); mji_mulMatTVec3(res, rotnew2old, tran);
mju_mulMatTVec3(res+3, rotnew2old, tran+3); mji_mulMatTVec3(res+3, rotnew2old, tran+3);
} }
// otherwise copy // otherwise copy
else { 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 // make yaxis orthogonal to xaxis
mju_scl3(tmp, frame, mju_dot3(frame, frame+3)); mji_scl3(tmp, frame, mju_dot3(frame, frame+3));
mju_subFrom3(frame+3, tmp); mji_subFrom3(frame+3, tmp);
mju_normalize3(frame+3); mju_normalize3(frame+3);
// zaxis = cross(xaxis, yaxis) // 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);
} }