chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Allow in-place computation for mju_mulQuat and mju_mulQuatAxis

Browse Source 
PiperOrigin-RevId: 471222168
Change-Id: If43210636cf20000b343ad1efd02836465380ebe
This commit is contained in:
Yuval Tassa
2022-08-31 04:08:28 -07:00
committed by Copybara-Service
parent 366e1c53d4
commit fdbbc8bbe0
3 changed files with 31 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+2 -1
View File
@@ -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
+9 -2
View File
@@ -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
+20 -8
View File
@@ -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];
}