diff --git a/STYLEGUIDE.md b/STYLEGUIDE.md index 5c97f0be..c0f12f52 100644 --- a/STYLEGUIDE.md +++ b/STYLEGUIDE.md @@ -116,7 +116,7 @@ before the `else`: else { mjtNum mat[9]; mju_quat2Mat(mat, quat); - mju_rotVecMat(res, vec, mat); + mju_mulMatVec3(res, mat, vec); } } ``` diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index d47ca1d7..569d3ee6 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3102,6 +3102,24 @@ mju_dist3 Return Cartesian distance between 3D vectors pos1 and pos2. +.. _mju_mulMatVec3: + +mju_mulMatVec3 +~~~~~~~~~~~~~~ + +.. mujoco-include:: mju_mulMatVec3 + +Multiply 3-by-3 matrix by vector: res = mat * vec. + +.. _mju_mulMatTVec3: + +mju_mulMatTVec3 +~~~~~~~~~~~~~~~ + +.. mujoco-include:: mju_mulMatTVec3 + +Multiply transposed 3-by-3 matrix by vector: res = mat' * vec. + .. _mju_rotVecMat: mju_rotVecMat @@ -3109,7 +3127,7 @@ mju_rotVecMat .. mujoco-include:: mju_rotVecMat -Multiply vector by 3D rotation matrix: res = mat * vec. +Deprecated, use mju_mulMatVec3(res, mat, vec). .. _mju_rotVecMatT: @@ -3118,7 +3136,7 @@ mju_rotVecMatT .. mujoco-include:: mju_rotVecMatT -Multiply vector by transposed 3D rotation matrix: res = mat' * vec. +Deprecated, use mju_mulMatTVec3(res, mat, vec). .. _mju_cross: diff --git a/doc/changelog.rst b/doc/changelog.rst index f643f499..babd164c 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -23,12 +23,14 @@ General 3. Added :ref:`maxhullvert`, the maximum number of vertices in a mesh's convex hull. 4. Added :ref:`mj_setKeyframe` for saving the current state into a model keyframe. 5. Added support for ``ball`` joints in the URDF parser ("spherical" in URDF). +6. Deprecated :ref:`mju_rotVecMat` and :ref:`mju_rotVecMatT` in favor of :ref:`mju_mulMatVec3` and + :ref:`mju_mulMatTVec3`. These functions names and argument ordering are more consistent with the rest of the API. MJX ~~~ -6. Added support for :ref:`elliptic friction cones`. -7. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. -8. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. +7. Added support for :ref:`elliptic friction cones`. +8. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. +9. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. Version 3.1.6 (Jun 3, 2024) --------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index 2f1fb622..1a92cc05 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3363,6 +3363,8 @@ mjtNum mju_normalize3(mjtNum vec[3]); mjtNum mju_norm3(const mjtNum vec[3]); mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]); mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); +void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); +void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 5c90a152..86d48cd7 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -970,10 +970,16 @@ MJAPI mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]); // Return Cartesian distance between 3D vectors pos1 and pos2. MJAPI mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); -// Multiply vector by 3D rotation matrix: res = mat * vec. +// Multiply 3-by-3 matrix by vector: res = mat * vec. +MJAPI void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); + +// Multiply transposed 3-by-3 matrix by vector: res = mat' * vec. +MJAPI void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); + +// Deprecated, use mju_mulMatVec3(res, mat, vec). MJAPI void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); -// Multiply vector by transposed 3D rotation matrix: res = mat' * vec. +// Deprecated, use mju_mulMatTVec3(res, mat, vec). MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); // Compute cross-product: res = cross(a, b). diff --git a/introspect/functions.py b/introspect/functions.py index 1e5875aa..0658ba1d 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -6130,6 +6130,64 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Return Cartesian distance between 3D vectors pos1 and pos2.', )), + ('mju_mulMatVec3', + FunctionDecl( + name='mju_mulMatVec3', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='res', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='mat', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(9,), + ), + ), + FunctionParameterDecl( + name='vec', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(3,), + ), + ), + ), + doc='Multiply 3-by-3 matrix by vector: res = mat * vec.', + )), + ('mju_mulMatTVec3', + FunctionDecl( + name='mju_mulMatTVec3', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='res', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='mat', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(9,), + ), + ), + FunctionParameterDecl( + name='vec', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(3,), + ), + ), + ), + doc="Multiply transposed 3-by-3 matrix by vector: res = mat' * vec.", + )), ('mju_rotVecMat', FunctionDecl( name='mju_rotVecMat', @@ -6157,7 +6215,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Multiply vector by 3D rotation matrix: res = mat * vec.', + doc='Deprecated, use mju_mulMatVec3(res, mat, vec).', )), ('mju_rotVecMatT', FunctionDecl( @@ -6186,7 +6244,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc="Multiply vector by transposed 3D rotation matrix: res = mat' * vec.", # pylint: disable=line-too-long + doc='Deprecated, use mju_mulMatTVec3(res, mat, vec).', )), ('mju_cross', FunctionDecl( diff --git a/mjx/requirements.txt b/mjx/requirements.txt index 7db3f624..2a371aae 100644 --- a/mjx/requirements.txt +++ b/mjx/requirements.txt @@ -33,7 +33,8 @@ jaxlib==0.4.18; python_version >= '3.9' \ --hash=sha256:6cb20bbbdafd90e71ad0deb9295519a0175c108c8c557b84fb9fe94f751daee4 \ --hash=sha256:116a0d6aedd3e856b52493d7e392fb1b40952b84fb72448fde1c1ab5687db667 \ --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 \ - --hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e + --hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e \ + --hash=sha256:b35ec08984e2aa5e96ba3f3f8b88e90dee0283649e037f213dec8e85638fa17d pip==23.3.1 \ --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b pytest==7.4.2 \ @@ -68,7 +69,8 @@ scipy==1.11.3; python_version >= '3.9' \ --hash=sha256:5305792c7110e32ff155aed0df46aa60a60fc6e52cd4ee02cdeb67eaccd5356e \ --hash=sha256:a63d1ec9cadecce838467ce0631c17c15c7197ae61e49429434ba01d618caa83 \ --hash=sha256:715c9966eb8906bc67e450e962bd07a5254420077178f98258904da4004a172f \ - --hash=sha256:d4d88a6fc091614b842a739b3db6ae15f95c77b308113bd6daefd4b05539b103 + --hash=sha256:d4d88a6fc091614b842a739b3db6ae15f95c77b308113bd6daefd4b05539b103 \ + --hash=sha256:cf0dbc4d3fe3107358868a60f263c9d8c2e9ba5de8a934cac4164124f727e6ca setuptools==68.2.2 \ --hash=sha256:b454a35605876da60632df1a60f736524eb73cc47bbc9f3f1ef1b644de74fd2a trimesh==4.0.0 \ diff --git a/plugin/sdf/sdf.cc b/plugin/sdf/sdf.cc index e3073393..56c84864 100644 --- a/plugin/sdf/sdf.cc +++ b/plugin/sdf/sdf.cc @@ -80,7 +80,7 @@ void SdfVisualizer::Visualize(const mjModel* m, const mjData* d, mjtNum* geom_quat = m->geom_quat + 4*g; mju_quat2Mat(geom_mat, geom_quat); mju_mulMatMatT(rotation, geom_xmat, geom_mat, 3, 3, 3); - mju_rotVecMat(offset, geom_pos, rotation); + mju_mulMatVec3(offset, rotation, geom_pos); mju_sub3(offset, geom_xpos, offset); for (int i = 0; i < niter; i++) { @@ -97,9 +97,9 @@ void SdfVisualizer::Visualize(const mjModel* m, const mjData* d, mjvGeom* thisgeom = scn->geoms + scn->ngeom; mjtNum* p1 = points + (tot + (k == 0 ? (n-1) * j : j))*3; mjtNum* p2 = points + (tot + j + 1)*3; - mju_rotVecMat(from, p1, rotation); + mju_mulMatVec3(from, rotation, p1); mju_addTo3(from, offset); - mju_rotVecMat(to, p2, rotation); + mju_mulMatVec3(to, rotation, p2); mju_addTo3(to, offset); if (k == 0) { float rgba[4] = {static_cast(j > 0), 0, diff --git a/plugin/sensor/touch_grid.cc b/plugin/sensor/touch_grid.cc index 4e58fee3..e9a6402b 100644 --- a/plugin/sensor/touch_grid.cc +++ b/plugin/sensor/touch_grid.cc @@ -306,10 +306,10 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { // Note that contact.frame is column major. mjtNum tmp_force[6], tmp1[3]; mj_contactForce(m, d, i, tmp_force); - mju_rotVecMatT(tmp1, tmp_force, d->contact[i].frame); - mju_rotVecMatT(forces + 6*contact, tmp1, site_mat); - mju_rotVecMatT(tmp1, tmp_force + 3, d->contact[i].frame); - mju_rotVecMatT(forces + 6*contact + 3, tmp1, site_mat); + mju_mulMatTVec3(tmp1, d->contact[i].frame, tmp_force); + mju_mulMatTVec3(forces + 6*contact, site_mat, tmp1); + mju_mulMatTVec3(tmp1, d->contact[i].frame, tmp_force + 3); + mju_mulMatTVec3(forces + 6*contact + 3, site_mat, tmp1); // Forces point from the smaller to larger body, so flip sign if // the parent body has smaller id. @@ -324,7 +324,7 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { // Get position, rotate into contact frame. mjtNum tmp2[3]; mju_sub3(tmp1, d->contact[i].pos, site_pos); - mju_rotVecMatT(tmp2, tmp1, site_mat); + mju_mulMatTVec3(tmp2, site_mat, tmp1); // Transform to spherical coordinates, copy into positions array. CartesianToSpherical(tmp2, tmp1); @@ -442,7 +442,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, 0.5*(y_edges[j+1]+y_edges[j]), dist*(1-kRelativeThickness)}; SphericalToCartesian(aer, pos); - mju_rotVecMat(pos, pos, site_mat); + mju_mulMatVec3(pos, site_mat, pos); mju_addTo3(pos, site_pos); // orientation diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index c636f8e1..7582ac70 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -698,6 +698,8 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index 3b1c9d1c..51471332 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -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); diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index f873e612..54f72ad5 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -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); diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 194def86..01660b52 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -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]); } diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index 49065e54..262548a6 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -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); diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 5723dafb..391e8c50 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -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++; diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 0916ab22..6e47d384 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -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]); } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 9b718448..3dfbb09b 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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; diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index a713baea..73c64b0f 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -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); } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 6fae24c6..8412f8f5 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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, diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index f3fb2705..8c341825 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -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 diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index c7150255..9fe2c2c3 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -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 diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index ec936192..92d47ae2 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -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); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index e7a02619..526a1334 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -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); } diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index 65c60c8d..bb920b86 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -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]; diff --git a/src/engine/engine_util_blas.h b/src/engine/engine_util_blas.h index 5bbfcd04..eaac6360 100644 --- a/src/engine/engine_util_blas.h +++ b/src/engine/engine_util_blas.h @@ -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 diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 3903d68b..3564e9b1 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -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]}; diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 1058087c..e2bdc8e7 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -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 diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 604a97b6..b485d46a 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -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 diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index a38125e9..66177c4f 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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 diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 61e8265f..3d09d181 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -1271,7 +1271,7 @@ void mjCMesh::ApplyTransformations() { // process vertices for (int i=0; i < nvert(); i++) { mjtNum p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - mju_rotVecMatT(p1, p0, mat); + mju_mulMatTVec3(p1, mat, p0); vert_[3*i] = (float) p1[0]; vert_[3*i+1] = (float) p1[1]; vert_[3*i+2] = (float) p1[2]; @@ -1280,7 +1280,7 @@ void mjCMesh::ApplyTransformations() { // process normals for (int i=0; i < nnormal(); i++) { mjtNum n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; - mju_rotVecMatT(n1, n0, mat); + mju_mulMatTVec3(n1, mat, n0); normal_[3*i] = (float) n1[0]; normal_[3*i+1] = (float) n1[1]; normal_[3*i+2] = (float) n1[2]; diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index 65d85545..87297004 100644 --- a/test/engine/engine_ray_test.cc +++ b/test/engine/engine_ray_test.cc @@ -296,8 +296,8 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid, mjtNum lpnt[3], lvec[3]; const mjtNum* pos = d->geom_xpos+3*geomid; const mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]}; - mju_rotVecMatT(lpnt, dif, d->geom_xmat+9*geomid); - mju_rotVecMatT(lvec, vec, d->geom_xmat+9*geomid); + mju_mulMatTVec3(lpnt, d->geom_xmat+9*geomid, dif); + mju_mulMatTVec3(lvec, d->geom_xmat+9*geomid, vec); // construct basis vectors of normal plane mjtNum b0[3] = {1, 1, 1}, b1[3]; diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index 8d968f39..159db210 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/test/engine/engine_util_spatial_test.cc @@ -115,7 +115,7 @@ void RotVecQuatWithMatrix(mjtNum res[3], const mjtNum vec[3], } else { mjtNum mat[9]; mju_quat2Mat(mat, quat); - mju_rotVecMat(res, vec, mat); + mju_mulMatVec3(res, mat, vec); } } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 62559128..2e791f25 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -7052,6 +7052,12 @@ public static unsafe extern double mju_dot3(double* vec1, double* vec2); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern double mju_dist3(double* pos1, double* pos2); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mju_mulMatVec3(double* res, double* mat, double* vec); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mju_mulMatTVec3(double* res, double* mat, double* vec); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_rotVecMat(double* res, double* vec, double* mat);