diff --git a/doc/changelog.rst b/doc/changelog.rst index ab0efdee..77d7706b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -49,7 +49,8 @@ General - Added ``mjv_defaultFreeCamera`` which sets the default free camera, respecting the above attributes. - ``simulate`` now supports taking a screenshot via a button in the File section or via ``Ctrl-P``. - Added a disable flag for sensors. - +- :ref:`mju_mulQuat` and :ref:`mju_mulQuatAxis` support in place computation. For example + |br| ``mju_mulQuat(a, a, b);`` sets the quaternion ``a`` equal to the product of ``a`` and ``b``. Deleted/deprecated features ^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/includes/macros.rst b/doc/includes/macros.rst index 41f9c83b..4d9ca5de 100644 --- a/doc/includes/macros.rst +++ b/doc/includes/macros.rst @@ -1,5 +1,5 @@ .. - Macro for adding non-breaking spaces for indentation. + Macros for adding non-breaking spaces for indentation. .. |_| unicode:: 0xA0 0xA0 :trim: @@ -8,4 +8,11 @@ :trim: .. |_3| unicode:: 0xA0 0xA0 0xA0 0xA0 0xA0 0xA0 - :trim: \ No newline at end of file + :trim: + +.. + Macro for injecting a custom line break. + +.. |br| raw:: html + +
\ No newline at end of file diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index a52dffad..0eb3955f 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -58,20 +58,32 @@ void mju_negQuat(mjtNum res[4], const mjtNum quat[4]) { // multiply quaternions void mju_mulQuat(mjtNum res[4], const mjtNum qa[4], const mjtNum qb[4]) { - res[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3]; - res[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2]; - res[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1]; - res[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0]; + mjtNum tmp[4] = { + qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3], + qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2], + qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1], + qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0] + }; + res[0] = tmp[0]; + res[1] = tmp[1]; + res[2] = tmp[2]; + res[3] = tmp[3]; } // multiply quaternion and axis void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3]) { - res[0] = - (quat[1]*axis[0] + quat[2]*axis[1] + quat[3]*axis[2]); - res[1] = quat[0]*axis[0] + quat[2]*axis[2] - quat[3]*axis[1]; - res[2] = quat[0]*axis[1] + quat[3]*axis[0] - quat[1]*axis[2]; - res[3] = quat[0]*axis[2] + quat[1]*axis[1] - quat[2]*axis[0]; + mjtNum tmp[4] = { + -quat[1]*axis[0] - quat[2]*axis[1] - quat[3]*axis[2], + quat[0]*axis[0] + quat[2]*axis[2] - quat[3]*axis[1], + quat[0]*axis[1] + quat[3]*axis[0] - quat[1]*axis[2], + quat[0]*axis[2] + quat[1]*axis[1] - quat[2]*axis[0] + }; + res[0] = tmp[0]; + res[1] = tmp[1]; + res[2] = tmp[2]; + res[3] = tmp[3]; }