<p>3D Rotations Functions</p>

80ab80c9-cd56-4284-b114-0d8953b1fa01 44 2 5 decimal

&[DATE] &[TIME] - &[FILENAME]

Euler angles sequence: ψ (yaw, around z), θ (pitch, around y), φ (roll, around x):

π - φ ≤ π ≤ π 2 / - θ ≤ π 2 / ≤ π - ψ ≤ π ≤ 31 sys

Quaternions:

e.0

e.x

e.y

e.z

e.0 2^e.x 2^+ e.y 2^+ e.z 2^+ 1 ≡

Angle and vector

E.x

E.y

E.z

Θ

E.x 2^E.y 2^+ E.z 2^+ 1 ≡

α R.x 1000 α cos α sin 0 α sin - α cos 33 mat :

α R.y α cos 0 α sin - 010 α sin 0 α cos 33 mat :

α R.z α cos α sin 0 α sin - α cos 000133 mat :

φ θ ψ Eul2Quat ψ 2 / cos θ 2 / cos * φ 2 / cos * ψ 2 / sin θ 2 / sin * φ 2 / sin * + ψ 2 / cos θ 2 / cos * φ 2 / sin * ψ 2 / sin θ 2 / sin * φ 2 / cos * - ψ 2 / cos θ 2 / sin * φ 2 / cos * ψ 2 / sin θ 2 / cos * φ 2 / sin * + ψ 2 / sin θ 2 / cos * φ 2 / cos * ψ 2 / cos θ 2 / sin * φ 2 / sin * - 41 mat :

e.0 e.x e.y e.z Quat2Mat e.0 2^e.x 2^+ e.y 2^- e.z 2^- 2 e.z e.0 * e.x e.y * + (* 2 e.x e.z * e.y e.0 * - (* 2 e.x e.y * e.z e.0 * - (* e.0 2^e.x 2^- e.y 2^+ e.z 2^- 2 e.x e.0 * e.z e.y * + (* 2 e.x e.z * e.y e.0 * + (* 2 e.y e.z * e.x e.0 * - (* e.0 2^e.x 2^- e.y 2^- e.z 2^+ 33 mat :

φ θ ψ Eul2Mat θ cos ψ cos * θ cos ψ sin * θ sin - φ sin θ sin * ψ cos * φ cos ψ sin * - φ sin θ sin * ψ sin * φ cos ψ cos * + φ sin θ cos * φ cos θ sin * ψ cos * φ sin ψ sin * + φ cos θ sin * ψ sin * φ sin ψ cos * - φ cos θ cos * 33 mat :

E.x E.y E.z Θ AngVec2Mat 12 Θ 2 / sin 2^* 1 E.x 2^- (* - 2 Θ 2 / sin 2^E.x * E.y * Θ 2 / sin Θ 2 / cos * E.z * + (* 2 Θ 2 / sin 2^E.x * E.z * Θ 2 / sin Θ 2 / cos * E.y * - (* 2 Θ 2 / sin 2^E.x * E.y * Θ 2 / sin Θ 2 / cos * E.z * - (* 12 Θ 2 / sin 2^* 1 E.y 2^- (* - 2 Θ 2 / sin 2^E.y * E.z * Θ 2 / sin Θ 2 / cos * E.x * + (* 2 Θ 2 / sin 2^E.x * E.z * Θ 2 / sin Θ 2 / cos * E.y * + (* 2 Θ 2 / sin 2^E.y * E.z * Θ 2 / sin Θ 2 / cos * E.x * - (* 12 Θ 2 / sin 2^* 1 E.z 2^- (* - 33 mat :

Θ E.x E.y E.z AngVec2Quat Θ 2 / cos E.x Θ 2 / sin * E.y Θ 2 / sin * E.z Θ 2 / sin * 41 mat :

AngVec2Eul

Eul2AngVec

to be defined

Mat2AngVec

e.0 e.x e.y e.z Quat2Eul e.0 2^e.x 2^- e.y 2^- e.z 2^+ (2 e.x e.0 * e.z e.y * + (* i * + arg 2 e.0 e.y * e.x e.z * - (* asin e.0 2^e.x 2^+ e.y 2^- e.z 2^- (2 e.z e.0 * e.x e.y * + (* i * + arg 31 mat :

A Mat2Eul A 33 el A 23 el i * + arg A 13 el asin - A 11 el A 12 el i * + arg 31 mat :

Particular cases θ = ±π/2 not handled

e.0 e.x e.y e.z Quat2AngVec 2 e.0 acos * e.x 1 e.0 2^- sqrt / e.y 1 e.0 2^- sqrt / e.z 1 e.0 2^- sqrt / 41 mat :

A Mat2Quat Q 1 A 11 el + A 22 el + A 33 el + 1 A 11 el + A 22 el - A 33 el - 1 A 11 el - A 22 el + A 33 el - 1 A 11 el - A 22 el - A 33 el + 41 mat SortIdx : Q 42 el 1 ≡ e.0 12 / Q 41 el sqrt * : e.x 14 e.0 * / A 23 el A 32 el - (* : e.y 14 e.0 * / A 31 el A 13 el - (* : e.z 14 e.0 * / A 12 el A 21 el - (* : 41 line Q 42 el 2 ≡ e.x 12 / Q 41 el sqrt * : e.0 14 e.x * / A 23 el A 32 el - (* : e.y 14 e.x * / A 12 el A 21 el + (* : e.z 14 e.x * / A 31 el A 13 el + (* : 41 line Q 42 el 3 ≡ e.y 12 / Q 41 el sqrt * : e.0 14 e.y * / A 31 el A 13 el - (* : e.x 14 e.y * / A 12 el A 21 el + (* : e.z 14 e.y * / A 23 el A 32 el + (* : 41 line Q 42 el 4 ≡ e.z 12 / Q 41 el sqrt * : e.0 14 e.z * / A 12 el A 21 el - (* : e.x 14 e.z * / A 31 el A 13 el + (* : e.y 14 e.z * / A 23 el A 32 el + (* : 41 line continue if if if if quat e.0 e.x e.y e.z 41 mat : 31 line :

Auxiliary function: given a vector Y, constructs a 2-columns matrix: the first column are the elements of Y ordered in ascending order; the second column are the original positional indices.

Y SortIdx k 1 Y rows range index k el k : for Y index augment 1 csort 21 line :

Quaternions from Euler Angles:

q1 30 deg * 20 deg * 45 deg * Eul2Quat :

q2 0 deg * 0 deg * 0 deg * Eul2Quat :

q3 30 deg * 90 deg * 20 deg * Eul2Quat :

q2 100041 mat

q3 0.7 0.06 0.7 0.06 - 41 mat

q1 0.9 0.17 0.25 0.32 41 mat

q3 k el 2^(k 14 sum 1

q1 k el 2^(k 14 sum 1

Rotation Matrix from Quaternions:

M1 q1 1 el q1 2 el q1 3 el q1 4 el Quat2Mat :

M2 q2 1 el q2 2 el q2 3 el q2 4 el Quat2Mat :

M3 q3 1 el q3 2 el q3 3 el q3 4 el Quat2Mat :

M1 0.66 0.66 0.34 - 0.49 - 0.73 0.47 0.56 0.14 - 0.81 33 mat

M2 10001000133 mat

M3 001 - 0.17 0.98 0 0.98 0.17 - 033 mat

The Rotation Matrix is orthonormal:

M1 transpose 0.66 0.49 - 0.56 0.66 0.73 0.14 - 0.34 - 0.47 0.81 33 mat

M1 1 -^0.66 0.49 - 0.56 0.66 0.73 0.14 - 0.34 - 0.47 0.81 33 mat

AngVec2Eul

Eul2AngVec

to be defined

M1 k 1 el 2^(k 13 sum 1

M1 k 2 el 2^(k 13 sum 1

M1 k 3 el 2^(k 13 sum 1

Mat2AngVec

Rotation Matrix from Euler Angles:

A1 30 deg * 20 deg * 45 deg * Eul2Mat :

A1 0.66 0.66 0.34 - 0.49 - 0.73 0.47 0.56 0.14 - 0.81 33 mat

30 deg * R.x 20 deg * R.y * 45 deg * R.z * 0.66 0.66 0.34 - 0.49 - 0.73 0.47 0.56 0.14 - 0.81 33 mat

...or as a sequence of rotations:

Euler Angles from Quaternions:

eul1 q1 1 el q1 2 el q1 3 el q1 4 el Quat2Eul :

eul2 M1 Mat2Eul :

eul3 q3 1 el q3 2 el q3 3 el q3 4 el Quat2Eul :

eul2 deg 30204531 mat

eul1 deg 30204531 mat

eul3 #

θ = ±π/2 is not handled yet

Angle Vector (versor) from Quaternions:

v1 q1 1 el q1 2 el q1 3 el q1 4 el Quat2AngVec :

v2 q2 1 el q2 2 el q2 3 el q2 4 el Quat2AngVec :

v3 q3 1 el q3 2 el q3 3 el q3 4 el Quat2AngVec :

v1 0.92 0.39 0.57 0.73 41 mat

v3 1.58 0.09 0.99 0.09 - 41 mat

v3 k 1 el 2^(k 24 sum 1

v1 k 1 el 2^(k 24 sum 1

φ = θ = ψ = 0 not handled

v3 1 el deg 90.44

v1 1 el deg 52.72

Quaternions from Rotation Matrix:

p1 A1 Mat2Quat :

p2 M2 Mat2Quat :

p3 M3 Mat2Quat :

p1 0.9 0.17 0.25 0.32 41 mat

q1 0.9 0.17 0.25 0.32 41 mat

p2 100041 mat

q2 100041 mat

p3 0.7 0.06 0.7 0.06 - 41 mat

q3 0.7 0.06 0.7 0.06 - 41 mat

compare to

Quaternions from Angle and Versor

v1 1 el v1 2 el v1 3 el v1 4 el AngVec2Quat 0.9 0.17 0.25 0.32 41 mat

q1 0.9 0.17 0.25 0.32 41 mat

compare to

v3 1 el v3 2 el v3 3 el v3 4 el AngVec2Quat 0.7 0.06 0.7 0.06 - 41 mat

q3 0.7 0.06 0.7 0.06 - 41 mat

compare to

Auxiliary function for indexed sorting. Given a vector Y, constructs a 2-columns matrix. the first column are the elements of Y ordered in ascending order; the second column are the original positional indices.

132201 - 51 mat SortIdx 1 - 511233220452 mat