chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
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];
}