chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 4 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_box.c
+16 -16
View File
@@ -48,7 +48,7 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
mjtNum dist, closest;
mju_sub3(tmp, pos1, pos2);
mju_rotVecMatT(center, tmp, mat2);
mju_mulMatTVec3(center, mat2, tmp);
mju_copy(clamped, center, 3);
mju_clampVec(clamped, size2, 3);
@@ -76,16 +76,16 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
mju_copy3(pos, center);
mju_addToScl3(pos, nearest, (size1[0] - closest) / 2);
mju_rotVecMat(con[0].frame, nearest, mat2);
mju_mulMatVec3(con[0].frame, mat2, nearest);
} else {
mju_addToScl3(deepest, tmp, size1[0]);
mju_zero3(pos);
mju_addToScl3(pos, clamped, 0.5);
mju_addToScl3(pos, deepest, 0.5);
mju_rotVecMat(con[0].frame, tmp, mat2);
mju_mulMatVec3(con[0].frame, mat2, tmp);
}
mju_rotVecMat(tmp, pos, mat2);
mju_mulMatVec3(tmp, mat2, pos);
mju_add3(con[0].pos, tmp, pos2);
con[0].dist = dist - size1[0];
mju_zero3(con[0].frame + 3);
@@ -153,13 +153,13 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
secondpos = -4; // initialize to no 2nd contact (valid values are between -1 and 1)
mju_sub3(tmp1, pos1, pos2); // bring capsule to box-local frame (center's box is at (0,0,0))
mju_rotVecMatT(pos, tmp1, mat2); // and axis parralel to world
mju_mulMatTVec3(pos, mat2, tmp1); // and axis parralel to world
tmp1[0] = mat1[2]; // capsule's axis
tmp1[1] = mat1[5];
tmp1[2] = mat1[8];
mju_rotVecMatT(axis, tmp1, mat2); // do the same for the capsule axis
mju_mulMatTVec3(axis, mat2, tmp1); // do the same for the capsule axis
mju_scl3(halfaxis, axis, halflength); // scale to get actual capsule half-axis
axisdir = 0;
@@ -576,7 +576,7 @@ skip:
// create sphere in original orientation at first contact point
mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, bestsegmentpos);
mju_rotVecMat(tmp2, tmp1, mat2);
mju_mulMatVec3(tmp2, mat2, tmp1);
mju_addTo3(tmp2, pos2);
// collide with
@@ -586,7 +586,7 @@ skip:
if (secondpos > -3) { // secondpos was modified
mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation
mju_rotVecMat(tmp2, tmp1, mat2);
mju_mulMatVec3(tmp2, mat2, tmp1);
mju_addTo3(tmp2, pos2);
n += mjraw_SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2);
}
@@ -633,10 +633,10 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
margin2 = margin * margin;
mju_sub3(tmp1, pos2, pos1);
mju_rotVecMatT(pos21, tmp1, mat1);
mju_mulMatTVec3(pos21, mat1, tmp1);
mju_sub3(tmp1, pos1, pos2);
mju_rotVecMatT(pos12, tmp1, mat2);
mju_mulMatTVec3(pos12, mat2, tmp1);
mju_mulMatTMat3(rot, mat1, mat2);
mju_transpose(rott, rot, 3, 3);
@@ -646,8 +646,8 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
for (i = 0; i < 9; i++)
rottabs[i] = fabs(rott[i]);
mju_rotVecMat(plen2, size2, rotabs);
mju_rotVecMatT(plen1, size1, rotabs);
mju_mulMatVec3(plen2, rotabs, size2);
mju_mulMatTVec3(plen1, rotabs, size1);
for (i = 0, penetration = margin; i < 3; i++)
penetration += size1[i] * 3 + size2[i] * 3;
@@ -974,7 +974,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
con[i].dist = points[i][2];
points[i][2] += hz;
mju_rotVecMat(tmp2, points[i], r);
mju_mulMatVec3(tmp2, r, points[i]);
mju_add3(con[i].pos, tmp2, p);
if (i)
@@ -1084,7 +1084,7 @@ edgeedge:
// mju_mulMatMat(r,rotmore,rot,3,3,3);
rotmatx(r, rot);
mju_rotVecMatT(tmp1, size1, rotmore);
mju_mulMatTVec3(tmp1, rotmore, size1);
for (i = 0; i < 3; i++)
s[i] = mju_abs(tmp1[i]);
@@ -1321,7 +1321,7 @@ edgeedge:
mju_mulMatMatT3(r, mat1, rotmore);
mju_rotVecMat(tmp1, rnorm, r);
mju_mulMatVec3(tmp1, r, rnorm);
mju_scl3(con[0].frame, tmp1, in ? -1 : 1);
mju_zero3(con[0].frame + 3);
@@ -1331,7 +1331,7 @@ edgeedge:
con[i].dist = depth[i];
points[i][2] += hz;
mju_rotVecMat(tmp2, points[i], r);
mju_mulMatVec3(tmp2, r, points[i]);
mju_add3(con[i].pos, tmp2, pos1);
src/engine/engine_collision_convex.c
+12 -12
View File
@@ -115,7 +115,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mjtNum res[3]; // result in geom local frame
// rotate dir to geom local frame
mju_rotVecMatT(dir, _dir->v, d->geom_xmat+9*g);
mju_mulMatTVec3(dir, d->geom_xmat+9*g, _dir->v);
// compute result according to geom type
switch ((mjtGeom) m->geom_type[g]) {
@@ -261,7 +261,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
}
// rotate result to global frame
mju_rotVecMat(vec->v, res, d->geom_xmat+9*g);
mju_mulMatVec3(vec->v, d->geom_xmat+9*g, res);
// add geom position
mju_addTo3(vec->v, d->geom_xpos+3*g);
@@ -338,7 +338,7 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
mju_sub3(rel, origin, xpos);
// displacement of origin due to rotation: vec = rot*rel - rel
mju_rotVecMat(vec, rel, rot);
mju_mulMatVec3(vec, rot, rel);
mju_subFrom3(vec, rel);
// correct xpos by subtracting displacement: xpos = xpos - vec
@@ -446,7 +446,7 @@ static int addplanemesh(mjContact* con, const float vertex[3],
const mjtNum first[3], mjtNum rbound) {
// compute point in global coordinates
mjtNum pnt[3], v[3] = {vertex[0], vertex[1], vertex[2]};
mju_rotVecMat(pnt, v, mat2);
mju_mulMatVec3(pnt, mat2, v);
mju_addTo3(pnt, pos2);
// skip if too close to first contact
@@ -517,7 +517,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
// express dir in geom local frame
mjtNum locdir[3];
mju_rotVecMatT(locdir, dir.v, d->geom_xmat+9*g);
mju_mulMatTVec3(locdir, d->geom_xmat+9*g, dir.v);
// inclusion threshold along locdir, relative to geom2 center
mju_sub3(dif, pos2, pos1);
@@ -797,8 +797,8 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
mju_rotVecMat(con[cnt].frame, dirccd.v, mat1);
mju_rotVecMat(con[cnt].pos, vecccd.v, mat1);
mju_mulMatVec3(con[cnt].frame, mat1, dirccd.v);
mju_mulMatVec3(con[cnt].pos, mat1, vecccd.v);
mju_addTo3(con[cnt].pos, pos1);
mju_zero3(con[cnt].frame+3);
@@ -979,8 +979,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
// map contact point and normal to local frame
mjtNum dif[3], pos[3], nrm[3];
mju_sub3(dif, con->pos, d->geom_xpos+3*gid[i]);
mju_rotVecMatT(pos, dif, mat);
mju_rotVecMatT(nrm, normal[i], mat);
mju_mulMatTVec3(pos, mat, dif);
mju_mulMatTVec3(nrm, mat, normal[i]);
// process according to type
switch (type[i]) {
@@ -1059,7 +1059,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
// normalize and map normal to global frame
if (processed[i]) {
mju_normalize3(nrm);
mju_rotVecMat(normal[i], nrm, mat);
mju_mulMatVec3(normal[i], mat, nrm);
}
}
}
@@ -1242,8 +1242,8 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
mju_rotVecMat(con[cnt].frame, dirccd.v, hmat);
mju_rotVecMat(con[cnt].pos, vecccd.v, hmat);
mju_mulMatVec3(con[cnt].frame, hmat, dirccd.v);
mju_mulMatVec3(con[cnt].pos, hmat, vecccd.v);
mju_addTo3(con[cnt].pos, hpos);
mju_zero3(con[cnt].frame+3);
src/engine/engine_collision_driver.c
+1 -1
View File
@@ -553,7 +553,7 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
for (int i=0; i < 2; i++) { // bounding boxes
for (int j=0; j < 3; j++) { // axes
if (xmat[i]) {
mju_rotVecMat(xcenter[i], aabb[i], xmat[i]);
mju_mulMatVec3(xcenter[i], xmat[i], aabb[i]);
} else {
mju_copy3(xcenter[i], aabb[i]);
}
src/engine/engine_collision_primitive.c
+1 -1
View File
@@ -219,7 +219,7 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d,
// get corner in global coordinates relative to box center
mjtNum corner[3];
mju_rotVecMat(corner, vec, mat2);
mju_mulMatVec3(corner, mat2, vec);
// compute distance to plane, skip if too far or pointing up
mjtNum ldist = mju_dot3(norm, corner);
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++;
src/engine/engine_core_constraint.c
+2 -2
View File
@@ -511,7 +511,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
case mjEQ_CONNECT: // connect bodies with ball joint
// find global points
for (int j=0; j < 2; j++) {
mju_rotVecMat(pos[j], data + 3*j, d->xmat + 9*id[j]);
mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j);
mju_addTo3(pos[j], d->xpos + 3*id[j]);
}
@@ -532,7 +532,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// find global points
for (int j=0; j < 2; j++) {
mjtNum* anchor = data + 3*(1-j);
mju_rotVecMat(pos[j], anchor, d->xmat + 9*id[j]);
mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor);
mju_addTo3(pos[j], d->xpos + 3*id[j]);
}
src/engine/engine_core_smooth.c
+11 -11
View File
@@ -88,7 +88,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
// apply fixed translation and rotation relative to parent
if (pid) {
mju_rotVecMat(xpos, bodypos, d->xmat+9*pid);
mju_mulMatVec3(xpos, d->xmat+9*pid, bodypos);
mju_addTo3(xpos, d->xpos+3*pid);
mju_mulQuat(xquat, d->xquat+4*pid, bodyquat);
} else {
@@ -464,7 +464,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// non-centered: map from local to global
else {
for (int i=vstart; i < vend; i++) {
mju_rotVecMat(d->flexvert_xpos+3*i, m->flex_vert+3*i, d->xmat+9*m->flex_vertbodyid[i]);
mju_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
@@ -1022,8 +1022,8 @@ void mj_transmission(const mjModel* m, mjData* d) {
// reference site undefined
if (m->actuator_trnid[2*i+1] == -1) {
// wrench: gear expressed in global frame
mju_rotVecMat(wrench, gear, d->site_xmat+9*id); // translation
mju_rotVecMat(wrench+3, gear+3, d->site_xmat+9*id); // rotation
mju_mulMatVec3(wrench, d->site_xmat+9*id, gear); // translation
mju_mulMatVec3(wrench+3, d->site_xmat+9*id, gear+3); // rotation
// moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation
@@ -1071,7 +1071,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
if (!mju_isZero(gear, 3)) {
// vec: site position in reference site frame
mju_sub3(vec, d->site_xpos+3*id, d->site_xpos+3*refid);
mju_rotVecMatT(vec, vec, d->site_xmat+9*refid);
mju_mulMatTVec3(vec, d->site_xmat+9*refid, vec);
// length: dot product with gear
length[i] += mju_dot3(vec, gear);
@@ -1092,7 +1092,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
}
// wrench: translational gear expressed in global frame
mju_rotVecMat(wrench, gear, d->site_xmat+9*refid);
mju_mulMatVec3(wrench, d->site_xmat+9*refid, gear);
// moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv);
@@ -1128,7 +1128,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
}
// wrench: rotational gear expressed in global frame
mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid);
mju_mulMatVec3(wrench, d->site_xmat+9*refid, gear+3);
// moment_tmp: global Jacobian projected on wrench, add to moment
if (!moment_tmp) moment_tmp = mj_stackAllocNum(d, nv);
@@ -1691,11 +1691,11 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
mju_scl3(d->subtree_linvel+3*i, body_vel+6*i+3, m->body_mass[i]);
// body angular momentum
mju_rotVecMatT(dv, body_vel+6*i, d->ximat+9*i);
mju_mulMatTVec3(dv, d->ximat+9*i, body_vel+6*i);
dv[0] *= m->body_inertia[3*i];
dv[1] *= m->body_inertia[3*i+1];
dv[2] *= m->body_inertia[3*i+2];
mju_rotVecMat(d->subtree_angmom+3*i, dv, d->ximat+9*i);
mju_mulMatVec3(d->subtree_angmom+3*i, d->ximat+9*i, dv);
}
// subtree linvel
@@ -1838,8 +1838,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
mj_contactForce(m, d, i, lfrc);
// cfrc = world-oriented torque:force vector (swap in the process)
mju_rotVecMatT(cfrc, lfrc+3, con->frame);
mju_rotVecMatT(cfrc+3, lfrc, con->frame);
mju_mulMatTVec3(cfrc, con->frame, lfrc+3);
mju_mulMatTVec3(cfrc+3, con->frame, lfrc);
// body 1
int k;
src/engine/engine_derivative.c
+1 -1
View File
@@ -320,7 +320,7 @@ void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale,
if (Dvel || Dscale) Dvel_[i] = b*eye[i] + c*cross[i] + d*outer[i];
}
if (Dvel) mju_copy(Dvel, Dvel_, 9);
if (Dscale) mju_rotVecMat(Dscale, vel, Dvel_);
if (Dscale) mju_mulMatVec3(Dscale, Dvel_, vel);
}
src/engine/engine_passive.c
+4 -4
View File
@@ -371,8 +371,8 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) {
mju_abs(lvel[2])*lvel[2]/64.0;
}
// rotate to global orientation: lfrc -> bfrc
mju_rotVecMat(bfrc, lfrc, d->ximat+9*i);
mju_rotVecMat(bfrc+3, lfrc+3, d->ximat+9*i);
mju_mulMatVec3(bfrc, d->ximat+9*i, lfrc);
mju_mulMatVec3(bfrc+3, d->ximat+9*i, lfrc+3);
// apply force and torque to body com
mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_fluid);
@@ -431,8 +431,8 @@ void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) {
mju_scl(lfrc, lfrc, geom_interaction_coef, 6);
// rotate to global orientation: lfrc -> bfrc
mju_rotVecMat(bfrc, lfrc, d->geom_xmat + 9*geomid);
mju_rotVecMat(bfrc+3, lfrc+3, d->geom_xmat + 9*geomid);
mju_mulMatVec3(bfrc, d->geom_xmat + 9*geomid, lfrc);
mju_mulMatVec3(bfrc+3, d->geom_xmat + 9*geomid, lfrc+3);
// apply force and torque to body com
mj_applyFT(m, d, bfrc+3, bfrc,
src/engine/engine_ray.c
+2 -2
View File
@@ -1122,7 +1122,7 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3],
// compute point in local coordinates of the box
mju_sub3(point, pnt, xpos);
mju_rotVecMatT(point, point, xmat);
mju_mulMatTVec3(point, xmat, point);
mju_subFrom3(point, aabb);
// check intersections
@@ -1238,7 +1238,7 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
vert[2] = (v&4 ? aabb[2]+aabb[5] : aabb[2]-aabb[5]);
// rotate to the world frame
mju_rotVecMat(box, vert, xmat);
mju_mulMatVec3(box, xmat, vert);
mju_addTo3(box, xpos);
// spherical coordinates
src/engine/engine_sensor.c
+4 -4
View File
@@ -328,12 +328,12 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref);
if (type == mjSENS_FRAMEPOS) {
mju_sub3(rvec, xpos, xpos_ref);
mju_rotVecMatT(d->sensordata+adr, rvec, xmat_ref);
mju_mulMatTVec3(d->sensordata+adr, xmat_ref, rvec);
} else {
// offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
int offset = type - mjSENS_FRAMEXAXIS;
mjtNum axis[3] = {xmat[offset], xmat[offset+3], xmat[offset+6]};
mju_rotVecMatT(d->sensordata+adr, axis, xmat_ref);
mju_mulMatTVec3(d->sensordata+adr, xmat_ref, axis);
}
}
break;
@@ -616,8 +616,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
mju_addTo3(rel_vel+3, cross);
// project into reference frame
mju_rotVecMatT(xvel, rel_vel, xmat_ref);
mju_rotVecMatT(xvel+3, rel_vel+3, xmat_ref);
mju_mulMatTVec3(xvel, xmat_ref, rel_vel);
mju_mulMatTVec3(xvel+3, xmat_ref, rel_vel+3);
}
// copy linear or angular component
src/engine/engine_setconst.c
+2 -2
View File
@@ -291,7 +291,7 @@ static void set0(mjModel* m, mjData* d) {
// data[3-5] = anchor position in body2 local frame
mju_subFrom3(pos, d->xpos+3*id2);
mju_rotVecMatT(m->eq_data+mjNEQDATA*i+3, pos, d->xmat+9*id2);
mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id2, pos);
}
// weld constraint
@@ -311,7 +311,7 @@ static void set0(mjModel* m, mjData* d) {
// data[3-5] = anchor position in body1 local frame
mju_subFrom3(pos, d->xpos+3*id1);
mju_rotVecMatT(m->eq_data+mjNEQDATA*i+3, pos, d->xmat+9*id1);
mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id1, pos);
// data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
mju_negQuat(quat, d->xquat+4*id1);
src/engine/engine_support.c
+1 -1
View File
@@ -1882,7 +1882,7 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
if (xpos && pos) {
// compute
if (sameframe == 0) {
mju_rotVecMat(xpos, pos, d->xmat+9*body);
mju_mulMatVec3(xpos, d->xmat+9*body, pos);
mju_addTo3(xpos, d->xpos+3*body);
}
src/engine/engine_util_blas.c
+18 -4
View File
@@ -152,8 +152,8 @@ mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
// multiply vector by 3D rotation matrix
void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
// multiply 3-by-3 matrix by vector
void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) {
mjtNum tmp[3] = {
mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2],
mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2],
@@ -166,8 +166,8 @@ void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
// multiply vector by transposed 3D rotation matrix
void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
// multiply transposed 3-by-3 matrix by vector
void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) {
mjtNum tmp[3] = {
mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2],
mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2],
@@ -180,6 +180,20 @@ void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
// multiply vector by 3D rotation matrix (deprecated)
void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
mju_mulMatVec3(res, mat, vec);
}
// multiply vector by transposed 3D rotation matrix (deprecated)
void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
mju_mulMatTVec3(res, mat, vec);
}
// multiply 3x3 matrices,
void mju_mulMatMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
src/engine/engine_util_blas.h
+8 -2
View File
@@ -103,10 +103,16 @@ MJAPI mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]);
// Cartesian distance between 3D vectors
MJAPI mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]);
// multiply vector by 3D rotation matrix
// multiply 3-by-3 matrix by vector
MJAPI void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]);
// multiply transposed 3-by-3 matrix by vector
MJAPI void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]);
// multiply vector by 3D rotation matrix (deprecated)
MJAPI void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]);
// multiply vector by transposed 3D rotation matrix
// multiply vector by transposed 3D rotation matrix (deprecated)
MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]);
// multiply 3x3 matrices
src/engine/engine_util_misc.c
+1 -1
View File
@@ -858,7 +858,7 @@ int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[
// vector from pos to point, projected to box frame
mjtNum vec[3] = {point[0]-pos[0], point[1]-pos[1], point[2]-pos[2]};
mju_rotVecMatT(vec, vec, mat);
mju_mulMatTVec3(vec, mat, vec);
// big: inflated box
mjtNum big[3] = {size[0], size[1], size[2]};
src/engine/engine_util_spatial.c
+2 -2
View File
@@ -471,8 +471,8 @@ void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force,
// apply rotation if provided
if (rotnew2old) {
mju_rotVecMatT(res, tran, rotnew2old);
mju_rotVecMatT(res+3, tran+3, rotnew2old);
mju_mulMatTVec3(res, rotnew2old, tran);
mju_mulMatTVec3(res+3, rotnew2old, tran+3);
}
// otherwise copy
src/engine/engine_vis_interact.c
+2 -2
View File
@@ -550,7 +550,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
// compute selection point in world coordinates
mjtNum selpos[3];
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*sel);
mju_mulMatVec3(selpos, d->xmat+9*sel, pert->localpos);
mju_addTo3(selpos, d->xpos+3*sel);
// compute average spatial inertia at selection point
@@ -667,7 +667,7 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert)
if (((pert->active | pert->active2) & mjPERT_TRANSLATE)) {
// compute selection point in world coordinates
mjtNum selpos[3];
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*sel);
mju_mulMatVec3(selpos, d->xmat+9*sel, pert->localpos);
mju_addTo3(selpos, d->xpos+3*sel);
// displacement of selection point from reference point
src/engine/engine_vis_visualize.c
+9 -9
View File
@@ -668,7 +668,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// offset xpos with aabb center (not always at frame origin)
const mjtNum *center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
mjtNum pos[3];
mju_rotVecMat(pos, center, xmat);
mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos);
// set box color
@@ -755,7 +755,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// offset xpos with aabb center (not always at geom origin)
const mjtNum *center = m->bvh_aabb + 6*i;
mjtNum pos[3];
mju_rotVecMat(pos, center, xmat);
mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos);
START
@@ -830,7 +830,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
START
// compute selection point in world coordinates
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*pert->select);
mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos);
mju_addTo3(selpos, d->xpos+3*pert->select);
// construct geom
@@ -873,7 +873,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (m->body_bvhnum[i]) {
mjtNum* aabb = m->bvh_aabb+6*m->body_bvhadr[i];
mju_copy3(sz, aabb+3);
mju_rotVecMat(pos, aabb, d->ximat+9*i);
mju_mulMatVec3(pos, d->ximat+9*i, aabb);
}
// otherwise box of size meansize
@@ -946,7 +946,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
int i=0;
// compute selection point in world coordinates
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*pert->select);
mju_mulMatVec3(selpos, d->xmat+9*pert->select, pert->localpos);
mju_addTo3(selpos, d->xpos+3*pert->select);
START
@@ -2030,9 +2030,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (d->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) {
// compute endpoints in global coordinates
int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD), d->xmat+9*j);
mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD));
mju_addTo3(vec, d->xpos+3*j);
mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_CONNECT), d->xmat+9*k);
mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_CONNECT));
mju_addTo3(end, d->xpos+3*k);
// construct geom
@@ -2636,7 +2636,7 @@ void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, cons
// compute translation
mjtNum translate[3];
mju_rotVecMat(translate, bindpos, rotate);
mju_mulMatVec3(translate, rotate, bindpos);
mju_sub3(translate, d->xpos+3*bodyid, translate);
// process all bone vertices
@@ -2656,7 +2656,7 @@ void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, cons
// transform
mjtNum pos1[3];
mju_rotVecMat(pos1, pos, rotate);
mju_mulMatVec3(pos1, rotate, pos);
mju_addTo3(pos1, translate);
// accumulate position