Skip to content

Instantly share code, notes, and snippets.

@alexandre-janniaux
Last active June 16, 2019 22:08
Show Gist options
  • Save alexandre-janniaux/bdb0dc3f25a31dd76f2353022a6209d2 to your computer and use it in GitHub Desktop.
Save alexandre-janniaux/bdb0dc3f25a31dd76f2353022a6209d2 to your computer and use it in GitHub Desktop.
⎡ ⎛ 2 2⎞ ⎤
⎢ 2⋅⎝gy + gz ⎠ 2⋅(-gw⋅gz + gx⋅gy) 2⋅(gw⋅gy + gx⋅gz) ⎥
⎢- ───────────────────── + 1 ───────────────────── ───────────────────── ⎥
⎢ 2 2 2 2 2 2 2 2 2 2 2 2 ⎥
⎢ gw + gx + gy + gz gw + gx + gy + gz gw + gx + gy + gz ⎥
⎢ ⎥
⎡sin(pitch)⋅sin(roll)⋅sin(yaw) + cos(roll)⋅cos(yaw) sin(roll)⋅cos(pitch) sin(pitch)⋅sin(roll)⋅cos(yaw) - sin(yaw)⋅cos(roll)⎤ ⎢ ⎛ 2 2⎞ ⎥
⎢ ⎥ ⎢ 2⋅(gw⋅gz + gx⋅gy) 2⋅⎝gx + gz ⎠ 2⋅(-gw⋅gx + gy⋅gz) ⎥
⎢sin(pitch)⋅sin(yaw)⋅cos(roll) - sin(roll)⋅cos(yaw) cos(pitch)⋅cos(roll) sin(pitch)⋅cos(roll)⋅cos(yaw) + sin(roll)⋅sin(yaw)⎥ = ⎢ ───────────────────── - ───────────────────── + 1 ───────────────────── ⎥
⎢ ⎥ ⎢ 2 2 2 2 2 2 2 2 2 2 2 2 ⎥
⎣ sin(yaw)⋅cos(pitch) -sin(pitch) cos(pitch)⋅cos(yaw) ⎦ ⎢ gw + gx + gy + gz gw + gx + gy + gz gw + gx + gy + gz ⎥
⎢ ⎥
⎢ ⎛ 2 2⎞ ⎥
⎢ 2⋅(-gw⋅gy + gx⋅gz) 2⋅(gw⋅gx + gy⋅gz) 2⋅⎝gx + gy ⎠ ⎥
⎢ ───────────────────── ───────────────────── - ───────────────────── + 1⎥
⎢ 2 2 2 2 2 2 2 2 2 2 2 2 ⎥
⎣ gw + gx + gy + gz gw + gx + gy + gz gw + gx + gy + gz ⎦
⎡⎛ ⎛ 2⋅(gw⋅gy - gx⋅gz) ⎞ ⎛ 2⋅(gw⋅gx + gy⋅gz) ⎞ ⎛ 2⋅(gw⋅gz - gx⋅gy)
⎢⎜π - asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟, asin⎜─────────────────────⎟ + π, π - asin⎜──────────────────────────────────────────────────────────────────────────────────────────
⎢⎜ ⎜ _______________________________________________________________________________________________ ⎟ ⎜ 2 2 2 2⎟ ⎜ ____________________________________________________________________________________
⎢⎜ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟ ⎝gw + gx + gy + gz ⎠ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2
⎢⎜ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz
⎢⎜ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────
⎢⎜ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2
⎣⎝ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy
⎞⎞ ⎛ ⎛ 2⋅(gw⋅gy - gx⋅gz) ⎞ ⎛ 2⋅(gw⋅gx + gy⋅gz) ⎞ ⎛ 2⋅
───────────────────────────────────⎟⎟, ⎜π - asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟, asin⎜─────────────────────⎟ + π, asin⎜────────────────────────────────────────────────────────
___________ ⎟⎟ ⎜ ⎜ _______________________________________________________________________________________________ ⎟ ⎜ 2 2 2 2⎟ ⎜ __________________________________________________
2⎞ ⎟⎟ ⎜ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟ ⎝gw + gx + gy + gz ⎠ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛
+ gz ⎠ ⎛ 2 2 2 2⎞⎟⎟ ⎜ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝g
────────── ⋅⎝gw + gx + gy + gz ⎠⎟⎟ ⎜ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟ ⎜ ╱ ─────────────────────────────────────────────────
2 4 ⎟⎟ ⎜ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟ ⎜ ╱ 4 2 2 2 2 2 2 4
⋅gz + gz ⎠⎠ ⎝ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2
(gw⋅gz - gx⋅gy) ⎞⎞ ⎛ ⎛ 2⋅(gw⋅gy - gx⋅gz) ⎞ ⎛ 2⋅(gw⋅gx + gy⋅gz) ⎞ ⎛
─────────────────────────────────────────────────────────────────────⎟⎟, ⎜asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟ + π, -asin⎜─────────────────────⎟, asin⎜─────────────────────────
_____________________________________________ ⎟⎟ ⎜ ⎜ _______________________________________________________________________________________________ ⎟ ⎜ 2 2 2 2⎟ ⎜ ___________________
2 2 2 2⎞ ⎟⎟ ⎜ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟ ⎝gw + gx + gy + gz ⎠ ⎜ ╱ ⎛ 2
w + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟⎟ ⎜ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟ ⎜ ╱ ⎝gw - 2⋅gw⋅gx
──────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟⎟ ⎜ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟ ⎜ ╱ ──────────────────
2 2 2 2 4 2 2 4 ⎟⎟ ⎜ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟ ⎜ ╱ 4 2 2
⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠⎠ ⎝ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠ ⎝╲╱ gw + 2⋅gw ⋅gx +
2⋅(gw⋅gz - gx⋅gy) ⎞ ⎞ ⎛ ⎛ 2⋅(gw⋅gy - gx⋅gz) ⎞ ⎛ 2⋅(gw⋅gx + gy⋅gz)
────────────────────────────────────────────────────────────────────────────────────────────────────⎟ + π⎟, ⎜asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟ + π, -asin⎜───────────────────
____________________________________________________________________________ ⎟ ⎟ ⎜ ⎜ _______________________________________________________________________________________________ ⎟ ⎜ 2 2 2
2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟ ⎟ ⎜ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟ ⎝gw + gx + gy + g
+ gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟ ⎟ ⎜ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟
─────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟ ⎟ ⎜ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟
2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟ ⎟ ⎜ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟
2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠ ⎠ ⎝ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠
⎞ ⎛ 2⋅(gw⋅gz - gx⋅gy) ⎞⎞ ⎛ ⎛ 2⋅(gw⋅gy - gx⋅gz)
──⎟, -asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟⎟, ⎜-asin⎜────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────
2⎟ ⎜ _______________________________________________________________________________________________ ⎟⎟ ⎜ ⎜ _______________________________________________________________________________________________
z ⎠ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟⎟ ⎜ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞
⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟⎟ ⎜ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2
⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟⎟ ⎜ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz
⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟⎟ ⎜ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4
⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠⎠ ⎝ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz
⎞ ⎛ 2⋅(gw⋅gx + gy⋅gz) ⎞ ⎛ 2⋅(gw⋅gz - gx⋅gy) ⎞ ⎞ ⎛ ⎛ 2⋅(gw⋅gy - gx⋅gz)
─⎟, -asin⎜─────────────────────⎟, asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟ + π⎟, ⎜-asin⎜────────────────────────────────────────────────────────────────────────────────────────────
⎟ ⎜ 2 2 2 2⎟ ⎜ _______________________________________________________________________________________________ ⎟ ⎟ ⎜ ⎜ ______________________________________________________________________________________
⎟ ⎝gw + gx + gy + gz ⎠ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟ ⎟ ⎜ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2
⎞⎟ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟ ⎟ ⎜ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz +
⎠⎟ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟ ⎟ ⎜ ⎜ ╱ ─────────────────────────────────────────────────────────────────────────────────────
⎟ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟ ⎟ ⎜ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2
⎠ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠ ⎠ ⎝ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅g
⎞ ⎛ 2⋅(gw⋅gx + gy⋅gz) ⎞ ⎛ 2⋅(gw⋅gz - gx⋅gy) ⎞⎞ ⎛ ⎛ 2⋅(gw⋅gy -
─────────────────────────────────⎟, -asin⎜─────────────────────⎟, -asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟⎟, ⎜asin⎜────────────────────────────────────────────────────────────────
_________ ⎟ ⎜ 2 2 2 2⎟ ⎜ _______________________________________________________________________________________________ ⎟⎟ ⎜ ⎜ __________________________________________________________
2⎞ ⎟ ⎝gw + gx + gy + gz ⎠ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟⎟ ⎜ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2
gz ⎠ ⎛ 2 2 2 2⎞⎟ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟⎟ ⎜ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅g
──────── ⋅⎝gw + gx + gy + gz ⎠⎟ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟⎟ ⎜ ⎜ ╱ ─────────────────────────────────────────────────────────
2 4 ⎟ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟⎟ ⎜ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2
z + gz ⎠ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠⎠ ⎝ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy
gx⋅gz) ⎞ ⎛ 2⋅(gw⋅gx + gy⋅gz) ⎞ ⎛ 2⋅(gw⋅gz - gx⋅gy) ⎞⎞ ⎛ ⎛
─────────────────────────────────────────────────────────────⎟, asin⎜─────────────────────⎟ + π, π - asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟⎟, ⎜asin⎜──────────────────────────────
_____________________________________ ⎟ ⎜ 2 2 2 2⎟ ⎜ _______________________________________________________________________________________________ ⎟⎟ ⎜ ⎜ ________________________
2 2 2⎞ ⎟ ⎝gw + gx + gy + gz ⎠ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟⎟ ⎜ ⎜ ╱ ⎛ 2 2
w⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟⎟ ⎜ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx
──────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟⎟ ⎜ ⎜ ╱ ───────────────────────
2 2 4 2 2 4 ⎟ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟⎟ ⎜ ⎜ ╱ 4 2 2 2
+ 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠⎠ ⎝ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw
2⋅(gw⋅gy - gx⋅gz) ⎞ ⎛ 2⋅(gw⋅gx + gy⋅gz) ⎞ ⎛ 2⋅(gw⋅gz - gx⋅gy) ⎞⎞⎤
───────────────────────────────────────────────────────────────────────────────────────────────⎟, asin⎜─────────────────────⎟ + π, asin⎜─────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────⎟⎟⎥
_______________________________________________________________________ ⎟ ⎜ 2 2 2 2⎟ ⎜ _______________________________________________________________________________________________ ⎟⎟⎥
2 2⎞ ⎛ 2 2 2 2⎞ ⎟ ⎝gw + gx + gy + gz ⎠ ⎜ ╱ ⎛ 2 2 2 2⎞ ⎛ 2 2 2 2⎞ ⎟⎟⎥
+ gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟ ⎜ ╱ ⎝gw - 2⋅gw⋅gx + gx + gy - 2⋅gy⋅gz + gz ⎠⋅⎝gw + 2⋅gw⋅gx + gx + gy + 2⋅gy⋅gz + gz ⎠ ⎛ 2 2 2 2⎞⎟⎟⎥
────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟ ⎜ ╱ ───────────────────────────────────────────────────────────────────────────────────────────── ⋅⎝gw + gx + gy + gz ⎠⎟⎟⎥
2 2 2 4 2 2 2 2 4 2 2 4 ⎟ ⎜ ╱ 4 2 2 2 2 2 2 4 2 2 2 2 4 2 2 4 ⎟⎟⎥
⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠ ⎝╲╱ gw + 2⋅gw ⋅gx + 2⋅gw ⋅gy + 2⋅gw ⋅gz + gx + 2⋅gx ⋅gy + 2⋅gx ⋅gz + gy + 2⋅gy ⋅gz + gz ⎠⎠⎦
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment