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

Deprecate mju_rotVecMat and mju_rotVecMatT in favor of mju_mulMatVec3 and mju_mulMatTVec3.

Browse Source 
These functions names and argument ordering are more consistent with the rest of the API.

PiperOrigin-RevId: 643788290
Change-Id: I783eda8021b80b82098e23ed95669b102bb82508
This commit is contained in:
Yuval Tassa
2024-06-16 10:01:31 -07:00
committed by Copybara-Service
parent 739512a0e4
commit 6067048537
33 changed files with 233 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_sdf.c
+14 -14
View File
@@ -191,17 +191,17 @@ mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjt
case mjSDFTYPE_SINGLE:
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat);
mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos);
return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]),
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]));
case mjSDFTYPE_MIDSURFACE:
mju_rotVecMat(y, x, s->relmat);
mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos);
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) -
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
case mjSDFTYPE_COLLISION:
mju_rotVecMat(y, x, s->relmat);
mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos);
mjtNum A = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
mjtNum B = geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
@@ -221,34 +221,34 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
switch (s->type) {
case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat);
mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos);
int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) >
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]) ? 0 : 1;
geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]);
if (i == 1) {
mju_rotVecMatT(gradient, gradient, s->relmat);
mju_mulMatTVec3(gradient, s->relmat, gradient);
}
break;
case mjSDFTYPE_MIDSURFACE:
mju_rotVecMat(y, x, s->relmat);
mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos);
geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
mju_normalize3(grad1);
geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
mju_rotVecMatT(grad2, grad2, s->relmat);
mju_mulMatTVec3(grad2, s->relmat, grad2);
mju_normalize3(grad2);
mju_sub3(gradient, grad1, grad2);
mju_normalize3(gradient);
break;
case mjSDFTYPE_COLLISION:
mju_rotVecMat(y, x, s->relmat);
mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos);
mjtNum A = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
mjtNum B = geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
mju_rotVecMatT(grad2, grad2, s->relmat);
mju_mulMatTVec3(grad2, s->relmat, grad2);
gradient[0] = grad1[0] + grad2[0];
gradient[1] = grad1[1] + grad2[1];
gradient[2] = grad1[2] + grad2[2];
@@ -487,7 +487,7 @@ static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
mjtNum candidate[3];
mjtNum r = mju_norm3(bvh+3);
mju_rotVecMat(candidate, bvh, rotation);
mju_mulMatVec3(candidate, rotation, bvh);
mju_addTo3(candidate, offset);
// check if inside the bounding box
@@ -613,7 +613,7 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
};
// transform local 1 (mesh) to local 2 (sdf)
mju_rotVecMat(corners+3*v, vec, rotation);
mju_mulMatVec3(corners+3*v, rotation, vec);
mju_addTo3(corners+3*v, offset);
}
@@ -694,7 +694,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
vec2[1] = (i&2 ? size2[1]+size2[4] : size2[1]-size2[4]);
vec2[2] = (i&4 ? size2[2]+size2[5] : size2[2]-size2[5]);
mju_rotVecMat(vec2, vec2, rotation1);
mju_mulMatVec3(vec2, rotation1, vec2);
mju_addTo3(vec2, offset1);
for (int k=0; k < 3; k++) {
@@ -753,10 +753,10 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
x[1] = aabb[1] + (aabb[4]-aabb[1]) * mju_Halton(j, 3);
x[2] = aabb[2] + (aabb[5]-aabb[2]) * mju_Halton(j, 5);
mju_rotVecMat(y, x, rotation2);
mju_mulMatVec3(y, rotation2, x);
mju_addTo3(y, offset2);
mju_rotVecMat(x, y, rotation12);
mju_mulMatVec3(x, rotation12, y);
mju_addTo3(x, offset12);
j++;