Allow in-place computation for mju_mulQuat and mju_mulQuatAxis

PiperOrigin-RevId: 471222168
Change-Id: If43210636cf20000b343ad1efd02836465380ebe

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
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^
 

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:
+   :trim:
+
+..
+   Macro for injecting a custom line break.
+
+.. |br| raw:: html
+
+  <br/>

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];
+  mjtNum tmp[4] = {
-  res[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2];
+    qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3],
-  res[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1];
+    qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2],
-  res[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0];
+    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]);
+  mjtNum tmp[4] = {
-  res[1] = quat[0]*axis[0] + quat[2]*axis[2] - quat[3]*axis[1];
+   -quat[1]*axis[0] - quat[2]*axis[1] - quat[3]*axis[2],
-  res[2] = quat[0]*axis[1] + quat[3]*axis[0] - quat[1]*axis[2];
+    quat[0]*axis[0] + quat[2]*axis[2] - quat[3]*axis[1],
-  res[3] = quat[0]*axis[2] + quat[1]*axis[1] - quat[2]*axis[0];
+    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];
 }