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 { else {
mjtNum mat[9]; mjtNum mat[9];
mju_quat2Mat(mat, quat); 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. 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:
mju_rotVecMat mju_rotVecMat
@@ -3109,7 +3127,7 @@ mju_rotVecMat
.. mujoco-include:: mju_rotVecMat .. mujoco-include:: mju_rotVecMat
Multiply vector by 3D rotation matrix: res = mat * vec. Deprecated, use mju_mulMatVec3(res, mat, vec).
.. _mju_rotVecMatT: .. _mju_rotVecMatT:
@@ -3118,7 +3136,7 @@ mju_rotVecMatT
.. mujoco-include:: 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: .. _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. 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. 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). 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 MJX
~~~ ~~~
6. Added support for :ref:`elliptic friction cones<option-cone>`. 7. 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 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. 9. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients.
Version 3.1.6 (Jun 3, 2024) 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_norm3(const mjtNum vec[3]);
mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]); mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]);
mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[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_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_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]); 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. // Return Cartesian distance between 3D vectors pos1 and pos2.
MJAPI mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); 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]); 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]); MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]);
// Compute cross-product: res = cross(a, b). // 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.', 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', ('mju_rotVecMat',
FunctionDecl( FunctionDecl(
name='mju_rotVecMat', 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', ('mju_rotVecMatT',
FunctionDecl( 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', ('mju_cross',
FunctionDecl( 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:6cb20bbbdafd90e71ad0deb9295519a0175c108c8c557b84fb9fe94f751daee4 \
--hash=sha256:116a0d6aedd3e856b52493d7e392fb1b40952b84fb72448fde1c1ab5687db667 \ --hash=sha256:116a0d6aedd3e856b52493d7e392fb1b40952b84fb72448fde1c1ab5687db667 \
--hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 \ --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 \
--hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e --hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e \
--hash=sha256:b35ec08984e2aa5e96ba3f3f8b88e90dee0283649e037f213dec8e85638fa17d
pip==23.3.1 \ pip==23.3.1 \
--hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b
pytest==7.4.2 \ pytest==7.4.2 \
@@ -68,7 +69,8 @@ scipy==1.11.3; python_version >= '3.9' \
--hash=sha256:5305792c7110e32ff155aed0df46aa60a60fc6e52cd4ee02cdeb67eaccd5356e \ --hash=sha256:5305792c7110e32ff155aed0df46aa60a60fc6e52cd4ee02cdeb67eaccd5356e \
--hash=sha256:a63d1ec9cadecce838467ce0631c17c15c7197ae61e49429434ba01d618caa83 \ --hash=sha256:a63d1ec9cadecce838467ce0631c17c15c7197ae61e49429434ba01d618caa83 \
--hash=sha256:715c9966eb8906bc67e450e962bd07a5254420077178f98258904da4004a172f \ --hash=sha256:715c9966eb8906bc67e450e962bd07a5254420077178f98258904da4004a172f \
--hash=sha256:d4d88a6fc091614b842a739b3db6ae15f95c77b308113bd6daefd4b05539b103 --hash=sha256:d4d88a6fc091614b842a739b3db6ae15f95c77b308113bd6daefd4b05539b103 \
--hash=sha256:cf0dbc4d3fe3107358868a60f263c9d8c2e9ba5de8a934cac4164124f727e6ca
setuptools==68.2.2 \ setuptools==68.2.2 \
--hash=sha256:b454a35605876da60632df1a60f736524eb73cc47bbc9f3f1ef1b644de74fd2a --hash=sha256:b454a35605876da60632df1a60f736524eb73cc47bbc9f3f1ef1b644de74fd2a
trimesh==4.0.0 \ 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; mjtNum* geom_quat = m->geom_quat + 4*g;
mju_quat2Mat(geom_mat, geom_quat); mju_quat2Mat(geom_mat, geom_quat);
mju_mulMatMatT(rotation, geom_xmat, geom_mat, 3, 3, 3); 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); mju_sub3(offset, geom_xpos, offset);
for (int i = 0; i < niter; i++) { 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; mjvGeom* thisgeom = scn->geoms + scn->ngeom;
mjtNum* p1 = points + (tot + (k == 0 ? (n-1) * j : j))*3; mjtNum* p1 = points + (tot + (k == 0 ? (n-1) * j : j))*3;
mjtNum* p2 = points + (tot + j + 1)*3; mjtNum* p2 = points + (tot + j + 1)*3;
mju_rotVecMat(from, p1, rotation); mju_mulMatVec3(from, rotation, p1);
mju_addTo3(from, offset); mju_addTo3(from, offset);
mju_rotVecMat(to, p2, rotation); mju_mulMatVec3(to, rotation, p2);
mju_addTo3(to, offset); mju_addTo3(to, offset);
if (k == 0) { if (k == 0) {
float rgba[4] = {static_cast<float>(j > 0), 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. // Note that contact.frame is column major.
mjtNum tmp_force[6], tmp1[3]; mjtNum tmp_force[6], tmp1[3];
mj_contactForce(m, d, i, tmp_force); mj_contactForce(m, d, i, tmp_force);
mju_rotVecMatT(tmp1, tmp_force, d->contact[i].frame); mju_mulMatTVec3(tmp1, d->contact[i].frame, tmp_force);
mju_rotVecMatT(forces + 6*contact, tmp1, site_mat); mju_mulMatTVec3(forces + 6*contact, site_mat, tmp1);
mju_rotVecMatT(tmp1, tmp_force + 3, d->contact[i].frame); mju_mulMatTVec3(tmp1, d->contact[i].frame, tmp_force + 3);
mju_rotVecMatT(forces + 6*contact + 3, tmp1, site_mat); mju_mulMatTVec3(forces + 6*contact + 3, site_mat, tmp1);
// Forces point from the smaller to larger body, so flip sign if // Forces point from the smaller to larger body, so flip sign if
// the parent body has smaller id. // 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. // Get position, rotate into contact frame.
mjtNum tmp2[3]; mjtNum tmp2[3];
mju_sub3(tmp1, d->contact[i].pos, site_pos); 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. // Transform to spherical coordinates, copy into positions array.
CartesianToSpherical(tmp2, tmp1); 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]), 0.5*(y_edges[j+1]+y_edges[j]),
dist*(1-kRelativeThickness)}; dist*(1-kRelativeThickness)};
SphericalToCartesian(aer, pos); SphericalToCartesian(aer, pos);
mju_rotVecMat(pos, pos, site_mat); mju_mulMatVec3(pos, site_mat, pos);
mju_addTo3(pos, site_pos); mju_addTo3(pos, site_pos);
// orientation // orientation
python/mujoco/functions.cc
+2
View File
@@ -698,6 +698,8 @@ PYBIND11_MODULE(_functions, pymodule) {
Def<traits::mju_norm3>(pymodule); Def<traits::mju_norm3>(pymodule);
Def<traits::mju_dot3>(pymodule); Def<traits::mju_dot3>(pymodule);
Def<traits::mju_dist3>(pymodule); Def<traits::mju_dist3>(pymodule);
Def<traits::mju_mulMatVec3>(pymodule);
Def<traits::mju_mulMatTVec3>(pymodule);
Def<traits::mju_rotVecMat>(pymodule); Def<traits::mju_rotVecMat>(pymodule);
Def<traits::mju_rotVecMatT>(pymodule); Def<traits::mju_rotVecMatT>(pymodule);
Def<traits::mju_cross>(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; mjtNum dist, closest;
mju_sub3(tmp, pos1, pos2); mju_sub3(tmp, pos1, pos2);
mju_rotVecMatT(center, tmp, mat2); mju_mulMatTVec3(center, mat2, tmp);
mju_copy(clamped, center, 3); mju_copy(clamped, center, 3);
mju_clampVec(clamped, size2, 3); mju_clampVec(clamped, size2, 3);
@@ -76,16 +76,16 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
mju_copy3(pos, center); mju_copy3(pos, center);
mju_addToScl3(pos, nearest, (size1[0] - closest) / 2); mju_addToScl3(pos, nearest, (size1[0] - closest) / 2);
mju_rotVecMat(con[0].frame, nearest, mat2); mju_mulMatVec3(con[0].frame, mat2, nearest);
} else { } else {
mju_addToScl3(deepest, tmp, size1[0]); mju_addToScl3(deepest, tmp, size1[0]);
mju_zero3(pos); mju_zero3(pos);
mju_addToScl3(pos, clamped, 0.5); mju_addToScl3(pos, clamped, 0.5);
mju_addToScl3(pos, deepest, 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); mju_add3(con[0].pos, tmp, pos2);
con[0].dist = dist - size1[0]; con[0].dist = dist - size1[0];
mju_zero3(con[0].frame + 3); mju_zero3(con[0].frame + 3);
@@ -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) 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_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[0] = mat1[2]; // capsule's axis
tmp1[1] = mat1[5]; tmp1[1] = mat1[5];
tmp1[2] = mat1[8]; 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 mju_scl3(halfaxis, axis, halflength); // scale to get actual capsule half-axis
axisdir = 0; axisdir = 0;
@@ -576,7 +576,7 @@ skip:
// create sphere in original orientation at first contact point // create sphere in original orientation at first contact point
mju_copy3(tmp1, pos); mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, bestsegmentpos); mju_addToScl3(tmp1, halfaxis, bestsegmentpos);
mju_rotVecMat(tmp2, tmp1, mat2); mju_mulMatVec3(tmp2, mat2, tmp1);
mju_addTo3(tmp2, pos2); mju_addTo3(tmp2, pos2);
// collide with // collide with
@@ -586,7 +586,7 @@ skip:
if (secondpos > -3) { // secondpos was modified if (secondpos > -3) { // secondpos was modified
mju_copy3(tmp1, pos); mju_copy3(tmp1, pos);
mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation
mju_rotVecMat(tmp2, tmp1, mat2); mju_mulMatVec3(tmp2, mat2, tmp1);
mju_addTo3(tmp2, pos2); mju_addTo3(tmp2, pos2);
n += mjraw_SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2); n += mjraw_SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2);
} }
@@ -633,10 +633,10 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
margin2 = margin * margin; margin2 = margin * margin;
mju_sub3(tmp1, pos2, pos1); mju_sub3(tmp1, pos2, pos1);
mju_rotVecMatT(pos21, tmp1, mat1); mju_mulMatTVec3(pos21, mat1, tmp1);
mju_sub3(tmp1, pos1, pos2); mju_sub3(tmp1, pos1, pos2);
mju_rotVecMatT(pos12, tmp1, mat2); mju_mulMatTVec3(pos12, mat2, tmp1);
mju_mulMatTMat3(rot, mat1, mat2); mju_mulMatTMat3(rot, mat1, mat2);
mju_transpose(rott, rot, 3, 3); mju_transpose(rott, rot, 3, 3);
@@ -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++) for (i = 0; i < 9; i++)
rottabs[i] = fabs(rott[i]); rottabs[i] = fabs(rott[i]);
mju_rotVecMat(plen2, size2, rotabs); mju_mulMatVec3(plen2, rotabs, size2);
mju_rotVecMatT(plen1, size1, rotabs); mju_mulMatTVec3(plen1, rotabs, size1);
for (i = 0, penetration = margin; i < 3; i++) for (i = 0, penetration = margin; i < 3; i++)
penetration += size1[i] * 3 + size2[i] * 3; penetration += size1[i] * 3 + size2[i] * 3;
@@ -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]; con[i].dist = points[i][2];
points[i][2] += hz; points[i][2] += hz;
mju_rotVecMat(tmp2, points[i], r); mju_mulMatVec3(tmp2, r, points[i]);
mju_add3(con[i].pos, tmp2, p); mju_add3(con[i].pos, tmp2, p);
if (i) if (i)
@@ -1084,7 +1084,7 @@ edgeedge:
// mju_mulMatMat(r,rotmore,rot,3,3,3); // mju_mulMatMat(r,rotmore,rot,3,3,3);
rotmatx(r, rot); rotmatx(r, rot);
mju_rotVecMatT(tmp1, size1, rotmore); mju_mulMatTVec3(tmp1, rotmore, size1);
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
s[i] = mju_abs(tmp1[i]); s[i] = mju_abs(tmp1[i]);
@@ -1321,7 +1321,7 @@ edgeedge:
mju_mulMatMatT3(r, mat1, rotmore); 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_scl3(con[0].frame, tmp1, in ? -1 : 1);
mju_zero3(con[0].frame + 3); mju_zero3(con[0].frame + 3);
@@ -1331,7 +1331,7 @@ edgeedge:
con[i].dist = depth[i]; con[i].dist = depth[i];
points[i][2] += hz; points[i][2] += hz;
mju_rotVecMat(tmp2, points[i], r); mju_mulMatVec3(tmp2, r, points[i]);
mju_add3(con[i].pos, tmp2, pos1); 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 mjtNum res[3]; // result in geom local frame
// rotate dir to 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 // compute result according to geom type
switch ((mjtGeom) m->geom_type[g]) { 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 // 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 // add geom position
mju_addTo3(vec->v, d->geom_xpos+3*g); 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); mju_sub3(rel, origin, xpos);
// displacement of origin due to rotation: vec = rot*rel - rel // displacement of origin due to rotation: vec = rot*rel - rel
mju_rotVecMat(vec, rel, rot); mju_mulMatVec3(vec, rot, rel);
mju_subFrom3(vec, rel); mju_subFrom3(vec, rel);
// correct xpos by subtracting displacement: xpos = xpos - vec // correct xpos by subtracting displacement: xpos = xpos - vec
@@ -446,7 +446,7 @@ static int addplanemesh(mjContact* con, const float vertex[3],
const mjtNum first[3], mjtNum rbound) { const mjtNum first[3], mjtNum rbound) {
// compute point in global coordinates // compute point in global coordinates
mjtNum pnt[3], v[3] = {vertex[0], vertex[1], vertex[2]}; 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); mju_addTo3(pnt, pos2);
// skip if too close to first contact // 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 // express dir in geom local frame
mjtNum locdir[3]; 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 // inclusion threshold along locdir, relative to geom2 center
mju_sub3(dif, pos2, pos1); mju_sub3(dif, pos2, pos1);
@@ -797,8 +797,8 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
!ccdVec3Eq(&dirccd, ccd_vec3_origin)) { !ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates // fill in contact data, transform to global coordinates
con[cnt].dist = -depth; con[cnt].dist = -depth;
mju_rotVecMat(con[cnt].frame, dirccd.v, mat1); mju_mulMatVec3(con[cnt].frame, mat1, dirccd.v);
mju_rotVecMat(con[cnt].pos, vecccd.v, mat1); mju_mulMatVec3(con[cnt].pos, mat1, vecccd.v);
mju_addTo3(con[cnt].pos, pos1); mju_addTo3(con[cnt].pos, pos1);
mju_zero3(con[cnt].frame+3); 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 // map contact point and normal to local frame
mjtNum dif[3], pos[3], nrm[3]; mjtNum dif[3], pos[3], nrm[3];
mju_sub3(dif, con->pos, d->geom_xpos+3*gid[i]); mju_sub3(dif, con->pos, d->geom_xpos+3*gid[i]);
mju_rotVecMatT(pos, dif, mat); mju_mulMatTVec3(pos, mat, dif);
mju_rotVecMatT(nrm, normal[i], mat); mju_mulMatTVec3(nrm, mat, normal[i]);
// process according to type // process according to type
switch (type[i]) { 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 // normalize and map normal to global frame
if (processed[i]) { if (processed[i]) {
mju_normalize3(nrm); 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)) { if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates // fill in contact data, transform to global coordinates
con[cnt].dist = -depth; con[cnt].dist = -depth;
mju_rotVecMat(con[cnt].frame, dirccd.v, hmat); mju_mulMatVec3(con[cnt].frame, hmat, dirccd.v);
mju_rotVecMat(con[cnt].pos, vecccd.v, hmat); mju_mulMatVec3(con[cnt].pos, hmat, vecccd.v);
mju_addTo3(con[cnt].pos, hpos); mju_addTo3(con[cnt].pos, hpos);
mju_zero3(con[cnt].frame+3); 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 i=0; i < 2; i++) { // bounding boxes
for (int j=0; j < 3; j++) { // axes for (int j=0; j < 3; j++) { // axes
if (xmat[i]) { if (xmat[i]) {
mju_rotVecMat(xcenter[i], aabb[i], xmat[i]); mju_mulMatVec3(xcenter[i], xmat[i], aabb[i]);
} else { } else {
mju_copy3(xcenter[i], aabb[i]); 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 // get corner in global coordinates relative to box center
mjtNum corner[3]; mjtNum corner[3];
mju_rotVecMat(corner, vec, mat2); mju_mulMatVec3(corner, mat2, vec);
// compute distance to plane, skip if too far or pointing up // compute distance to plane, skip if too far or pointing up
mjtNum ldist = mju_dot3(norm, corner); 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: case mjSDFTYPE_SINGLE:
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
case mjSDFTYPE_INTERSECTION: case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat); mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos); mju_addTo3(y, s->relpos);
return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]), 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])); geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]));
case mjSDFTYPE_MIDSURFACE: case mjSDFTYPE_MIDSURFACE:
mju_rotVecMat(y, x, s->relmat); mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos); mju_addTo3(y, s->relpos);
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) - 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]); geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
case mjSDFTYPE_COLLISION: case mjSDFTYPE_COLLISION:
mju_rotVecMat(y, x, s->relmat); mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos); mju_addTo3(y, s->relpos);
mjtNum A = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); 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]); 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) { switch (s->type) {
case mjSDFTYPE_INTERSECTION: case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat); mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos); mju_addTo3(y, s->relpos);
int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) > 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; 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]); geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]);
if (i == 1) { if (i == 1) {
mju_rotVecMatT(gradient, gradient, s->relmat); mju_mulMatTVec3(gradient, s->relmat, gradient);
} }
break; break;
case mjSDFTYPE_MIDSURFACE: case mjSDFTYPE_MIDSURFACE:
mju_rotVecMat(y, x, s->relmat); mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos); mju_addTo3(y, s->relpos);
geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
mju_normalize3(grad1); mju_normalize3(grad1);
geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]); 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_normalize3(grad2);
mju_sub3(gradient, grad1, grad2); mju_sub3(gradient, grad1, grad2);
mju_normalize3(gradient); mju_normalize3(gradient);
break; break;
case mjSDFTYPE_COLLISION: case mjSDFTYPE_COLLISION:
mju_rotVecMat(y, x, s->relmat); mju_mulMatVec3(y, s->relmat, x);
mju_addTo3(y, s->relpos); mju_addTo3(y, s->relpos);
mjtNum A = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); 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]); 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(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]); 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[0] = grad1[0] + grad2[0];
gradient[1] = grad1[1] + grad2[1]; gradient[1] = grad1[1] + grad2[1];
gradient[2] = grad1[2] + grad2[2]; 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 candidate[3];
mjtNum r = mju_norm3(bvh+3); mjtNum r = mju_norm3(bvh+3);
mju_rotVecMat(candidate, bvh, rotation); mju_mulMatVec3(candidate, rotation, bvh);
mju_addTo3(candidate, offset); mju_addTo3(candidate, offset);
// check if inside the bounding box // 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) // 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); 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[1] = (i&2 ? size2[1]+size2[4] : size2[1]-size2[4]);
vec2[2] = (i&4 ? size2[2]+size2[5] : size2[2]-size2[5]); 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); mju_addTo3(vec2, offset1);
for (int k=0; k < 3; k++) { 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[1] = aabb[1] + (aabb[4]-aabb[1]) * mju_Halton(j, 3);
x[2] = aabb[2] + (aabb[5]-aabb[2]) * mju_Halton(j, 5); 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_addTo3(y, offset2);
mju_rotVecMat(x, y, rotation12); mju_mulMatVec3(x, rotation12, y);
mju_addTo3(x, offset12); mju_addTo3(x, offset12);
j++; 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 case mjEQ_CONNECT: // connect bodies with ball joint
// find global points // find global points
for (int j=0; j < 2; j++) { 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]); mju_addTo3(pos[j], d->xpos + 3*id[j]);
} }
@@ -532,7 +532,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// find global points // find global points
for (int j=0; j < 2; j++) { for (int j=0; j < 2; j++) {
mjtNum* anchor = data + 3*(1-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]); 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 // apply fixed translation and rotation relative to parent
if (pid) { 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_addTo3(xpos, d->xpos+3*pid);
mju_mulQuat(xquat, d->xquat+4*pid, bodyquat); mju_mulQuat(xquat, d->xquat+4*pid, bodyquat);
} else { } else {
@@ -464,7 +464,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// non-centered: map from local to global // non-centered: map from local to global
else { else {
for (int i=vstart; i < vend; i++) { for (int i=vstart; i < vend; i++) {
mju_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]); 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 // reference site undefined
if (m->actuator_trnid[2*i+1] == -1) { if (m->actuator_trnid[2*i+1] == -1) {
// wrench: gear expressed in global frame // wrench: gear expressed in global frame
mju_rotVecMat(wrench, gear, d->site_xmat+9*id); // translation mju_mulMatVec3(wrench, d->site_xmat+9*id, gear); // translation
mju_rotVecMat(wrench+3, gear+3, d->site_xmat+9*id); // rotation mju_mulMatVec3(wrench+3, d->site_xmat+9*id, gear+3); // rotation
// moment: global Jacobian projected on wrench // moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation 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)) { if (!mju_isZero(gear, 3)) {
// vec: site position in reference site frame // vec: site position in reference site frame
mju_sub3(vec, d->site_xpos+3*id, d->site_xpos+3*refid); 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: dot product with gear
length[i] += mju_dot3(vec, 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 // 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 // moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); 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 // 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 // moment_tmp: global Jacobian projected on wrench, add to moment
if (!moment_tmp) moment_tmp = mj_stackAllocNum(d, nv); 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]); mju_scl3(d->subtree_linvel+3*i, body_vel+6*i+3, m->body_mass[i]);
// body angular momentum // 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[0] *= m->body_inertia[3*i];
dv[1] *= m->body_inertia[3*i+1]; dv[1] *= m->body_inertia[3*i+1];
dv[2] *= m->body_inertia[3*i+2]; dv[2] *= m->body_inertia[3*i+2];
mju_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 // subtree linvel
@@ -1838,8 +1838,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
mj_contactForce(m, d, i, lfrc); mj_contactForce(m, d, i, lfrc);
// cfrc = world-oriented torque:force vector (swap in the process) // cfrc = world-oriented torque:force vector (swap in the process)
mju_rotVecMatT(cfrc, lfrc+3, con->frame); mju_mulMatTVec3(cfrc, con->frame, lfrc+3);
mju_rotVecMatT(cfrc+3, lfrc, con->frame); mju_mulMatTVec3(cfrc+3, con->frame, lfrc);
// body 1 // body 1
int k; 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 || Dscale) Dvel_[i] = b*eye[i] + c*cross[i] + d*outer[i];
} }
if (Dvel) mju_copy(Dvel, Dvel_, 9); 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; mju_abs(lvel[2])*lvel[2]/64.0;
} }
// rotate to global orientation: lfrc -> bfrc // rotate to global orientation: lfrc -> bfrc
mju_rotVecMat(bfrc, lfrc, d->ximat+9*i); mju_mulMatVec3(bfrc, d->ximat+9*i, lfrc);
mju_rotVecMat(bfrc+3, lfrc+3, d->ximat+9*i); mju_mulMatVec3(bfrc+3, d->ximat+9*i, lfrc+3);
// apply force and torque to body com // apply force and torque to body com
mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_fluid); mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_fluid);
@@ -431,8 +431,8 @@ void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) {
mju_scl(lfrc, lfrc, geom_interaction_coef, 6); mju_scl(lfrc, lfrc, geom_interaction_coef, 6);
// rotate to global orientation: lfrc -> bfrc // rotate to global orientation: lfrc -> bfrc
mju_rotVecMat(bfrc, lfrc, d->geom_xmat + 9*geomid); mju_mulMatVec3(bfrc, d->geom_xmat + 9*geomid, lfrc);
mju_rotVecMat(bfrc+3, lfrc+3, d->geom_xmat + 9*geomid); mju_mulMatVec3(bfrc+3, d->geom_xmat + 9*geomid, lfrc+3);
// apply force and torque to body com // apply force and torque to body com
mj_applyFT(m, d, bfrc+3, bfrc, mj_applyFT(m, d, bfrc+3, bfrc,
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 // compute point in local coordinates of the box
mju_sub3(point, pnt, xpos); mju_sub3(point, pnt, xpos);
mju_rotVecMatT(point, point, xmat); mju_mulMatTVec3(point, xmat, point);
mju_subFrom3(point, aabb); mju_subFrom3(point, aabb);
// check intersections // 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]); vert[2] = (v&4 ? aabb[2]+aabb[5] : aabb[2]-aabb[5]);
// rotate to the world frame // rotate to the world frame
mju_rotVecMat(box, vert, xmat); mju_mulMatVec3(box, xmat, vert);
mju_addTo3(box, xpos); mju_addTo3(box, xpos);
// spherical coordinates // 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); get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref);
if (type == mjSENS_FRAMEPOS) { if (type == mjSENS_FRAMEPOS) {
mju_sub3(rvec, xpos, xpos_ref); mju_sub3(rvec, xpos, xpos_ref);
mju_rotVecMatT(d->sensordata+adr, rvec, xmat_ref); mju_mulMatTVec3(d->sensordata+adr, xmat_ref, rvec);
} else { } else {
// offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively // offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
int offset = type - mjSENS_FRAMEXAXIS; int offset = type - mjSENS_FRAMEXAXIS;
mjtNum axis[3] = {xmat[offset], xmat[offset+3], xmat[offset+6]}; 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; break;
@@ -616,8 +616,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
mju_addTo3(rel_vel+3, cross); mju_addTo3(rel_vel+3, cross);
// project into reference frame // project into reference frame
mju_rotVecMatT(xvel, rel_vel, xmat_ref); mju_mulMatTVec3(xvel, xmat_ref, rel_vel);
mju_rotVecMatT(xvel+3, rel_vel+3, xmat_ref); mju_mulMatTVec3(xvel+3, xmat_ref, rel_vel+3);
} }
// copy linear or angular component // 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 // data[3-5] = anchor position in body2 local frame
mju_subFrom3(pos, d->xpos+3*id2); 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 // weld constraint
@@ -311,7 +311,7 @@ static void set0(mjModel* m, mjData* d) {
// data[3-5] = anchor position in body1 local frame // data[3-5] = anchor position in body1 local frame
mju_subFrom3(pos, d->xpos+3*id1); 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 // data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
mju_negQuat(quat, d->xquat+4*id1); 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) { if (xpos && pos) {
// compute // compute
if (sameframe == 0) { 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); 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 // multiply 3-by-3 matrix by vector
void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) { void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) {
mjtNum tmp[3] = { mjtNum tmp[3] = {
mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2], mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2],
mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*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 // multiply transposed 3-by-3 matrix by vector
void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) { void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) {
mjtNum tmp[3] = { mjtNum tmp[3] = {
mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2], mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2],
mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*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, // multiply 3x3 matrices,
void mju_mulMatMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) { 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]; 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 // Cartesian distance between 3D vectors
MJAPI mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); 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]); 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]); MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]);
// multiply 3x3 matrices // 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 // vector from pos to point, projected to box frame
mjtNum vec[3] = {point[0]-pos[0], point[1]-pos[1], point[2]-pos[2]}; 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 // big: inflated box
mjtNum big[3] = {size[0], size[1], size[2]}; 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 // apply rotation if provided
if (rotnew2old) { if (rotnew2old) {
mju_rotVecMatT(res, tran, rotnew2old); mju_mulMatTVec3(res, rotnew2old, tran);
mju_rotVecMatT(res+3, tran+3, rotnew2old); mju_mulMatTVec3(res+3, rotnew2old, tran+3);
} }
// otherwise copy // 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 // compute selection point in world coordinates
mjtNum selpos[3]; 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); mju_addTo3(selpos, d->xpos+3*sel);
// compute average spatial inertia at selection point // 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)) { if (((pert->active | pert->active2) & mjPERT_TRANSLATE)) {
// compute selection point in world coordinates // compute selection point in world coordinates
mjtNum selpos[3]; 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); mju_addTo3(selpos, d->xpos+3*sel);
// displacement of selection point from reference point // 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) // offset xpos with aabb center (not always at frame origin)
const mjtNum *center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i; const mjtNum *center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
mjtNum pos[3]; mjtNum pos[3];
mju_rotVecMat(pos, center, xmat); mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos); mju_addTo3(pos, xpos);
// set box color // 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) // offset xpos with aabb center (not always at geom origin)
const mjtNum *center = m->bvh_aabb + 6*i; const mjtNum *center = m->bvh_aabb + 6*i;
mjtNum pos[3]; mjtNum pos[3];
mju_rotVecMat(pos, center, xmat); mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos); mju_addTo3(pos, xpos);
START START
@@ -830,7 +830,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
START START
// compute selection point in world coordinates // 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); mju_addTo3(selpos, d->xpos+3*pert->select);
// construct geom // construct geom
@@ -873,7 +873,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (m->body_bvhnum[i]) { if (m->body_bvhnum[i]) {
mjtNum* aabb = m->bvh_aabb+6*m->body_bvhadr[i]; mjtNum* aabb = m->bvh_aabb+6*m->body_bvhadr[i];
mju_copy3(sz, aabb+3); 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 // otherwise box of size meansize
@@ -946,7 +946,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
int i=0; int i=0;
// compute selection point in world coordinates // 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); mju_addTo3(selpos, d->xpos+3*pert->select);
START 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)) { if (d->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) {
// compute endpoints in global coordinates // compute endpoints in global coordinates
int j = m->eq_obj1id[i], k = m->eq_obj2id[i]; 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_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); mju_addTo3(end, d->xpos+3*k);
// construct geom // construct geom
@@ -2636,7 +2636,7 @@ void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, cons
// compute translation // compute translation
mjtNum translate[3]; mjtNum translate[3];
mju_rotVecMat(translate, bindpos, rotate); mju_mulMatVec3(translate, rotate, bindpos);
mju_sub3(translate, d->xpos+3*bodyid, translate); mju_sub3(translate, d->xpos+3*bodyid, translate);
// process all bone vertices // process all bone vertices
@@ -2656,7 +2656,7 @@ void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, cons
// transform // transform
mjtNum pos1[3]; mjtNum pos1[3];
mju_rotVecMat(pos1, pos, rotate); mju_mulMatVec3(pos1, rotate, pos);
mju_addTo3(pos1, translate); mju_addTo3(pos1, translate);
// accumulate position // accumulate position
src/user/user_mesh.cc
+2 -2
View File
@@ -1271,7 +1271,7 @@ void mjCMesh::ApplyTransformations() {
// process vertices // process vertices
for (int i=0; i < nvert(); i++) { for (int i=0; i < nvert(); i++) {
mjtNum p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; 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] = (float) p1[0];
vert_[3*i+1] = (float) p1[1]; vert_[3*i+1] = (float) p1[1];
vert_[3*i+2] = (float) p1[2]; vert_[3*i+2] = (float) p1[2];
@@ -1280,7 +1280,7 @@ void mjCMesh::ApplyTransformations() {
// process normals // process normals
for (int i=0; i < nnormal(); i++) { for (int i=0; i < nnormal(); i++) {
mjtNum n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; 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] = (float) n1[0];
normal_[3*i+1] = (float) n1[1]; normal_[3*i+1] = (float) n1[1];
normal_[3*i+2] = (float) n1[2]; 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]; mjtNum lpnt[3], lvec[3];
const mjtNum* pos = d->geom_xpos+3*geomid; const mjtNum* pos = d->geom_xpos+3*geomid;
const mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]}; 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_mulMatTVec3(lpnt, d->geom_xmat+9*geomid, dif);
mju_rotVecMatT(lvec, vec, d->geom_xmat+9*geomid); mju_mulMatTVec3(lvec, d->geom_xmat+9*geomid, vec);
// construct basis vectors of normal plane // construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3]; 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 { } else {
mjtNum mat[9]; mjtNum mat[9];
mju_quat2Mat(mat, quat); 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)] [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern double mju_dist3(double* pos1, double* pos2); 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)] [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mju_rotVecMat(double* res, double* vec, double* mat); public static unsafe extern void mju_rotVecMat(double* res, double* vec, double* mat);