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
STYLEGUIDE.md
+1 -1
View File
@@ -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);
}
}
```
doc/APIreference/functions.rst
+20 -2
View File
@@ -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:
doc/changelog.rst
+5 -3
View File
@@ -23,12 +23,14 @@ General
3. Added :ref:`maxhullvert<asset-mesh-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<option-cone>`.
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<option-cone>`.
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)
---------------------------
doc/includes/references.h
+2
View File
@@ -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]);
include/mujoco/mujoco.h
+8 -2
View File
@@ -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).
introspect/functions.py
+60 -2
View File
@@ -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(
mjx/requirements.txt
+4 -2
View File
@@ -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 \
plugin/sdf/sdf.cc
+3 -3
View File
@@ -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<float>(j > 0), 0,
plugin/sensor/touch_grid.cc
+6 -6
View File
@@ -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
python/mujoco/functions.cc
+2
View File
@@ -698,6 +698,8 @@ PYBIND11_MODULE(_functions, pymodule) {
Def<traits::mju_norm3>(pymodule);
Def<traits::mju_dot3>(pymodule);
Def<traits::mju_dist3>(pymodule);
Def<traits::mju_mulMatVec3>(pymodule);
Def<traits::mju_mulMatTVec3>(pymodule);
Def<traits::mju_rotVecMat>(pymodule);
Def<traits::mju_rotVecMatT>(pymodule);
Def<traits::mju_cross>(pymodule);
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
src/user/user_mesh.cc
+2 -2
View File
@@ -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];
test/engine/engine_ray_test.cc
+2 -2
View File
@@ -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];
test/engine/engine_util_spatial_test.cc
+1 -1
View File
@@ -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);
}
}
unity/Runtime/Bindings/MjBindings.cs
+6
View File
@@ -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);