Algorithms_in_C 1.0.0
Set of algorithms implemented in C.
Loading...
Searching...
No Matches
3D Quaternion operations
Collaboration diagram for 3D Quaternion operations:

Data Structures

struct  quaternion_
 a Quaternion type represented using a scalar \(w\) or \(q_0\) and a 3D vector \(\left(q_1,q_2,q_3\right)\) More...
 
struct  euler_
 3D Euler or Tait-Bryan angles (in radian) More...
 

Typedefs

typedef struct quaternion_ quaternion
 a Quaternion type represented using a scalar \(w\) or \(q_0\) and a 3D vector \(\left(q_1,q_2,q_3\right)\)
 
typedef struct euler_ euler
 3D Euler or Tait-Bryan angles (in radian)
 

Functions

quaternion quat_from_euler (const euler *in_euler)
 Function to convert given Euler angles to a quaternion. More...
 
euler euler_from_quat (const quaternion *in_quat)
 Function to convert given quaternion to Euler angles. More...
 
quaternion quaternion_multiply (const quaternion *in_quat1, const quaternion *in_quat2)
 Function to multiply two quaternions. More...
 

Detailed Description

Function Documentation

◆ euler_from_quat()

euler euler_from_quat ( const quaternion in_quat)

Function to convert given quaternion to Euler angles.

\begin{eqnarray*} \phi & = & \tan^{-1}\left[\frac{2\left(q_0q_1+q_2q_3\right)}{1-2\left(q_1^2+q_2^2\right)}\right]\\ \theta & = &-\sin^{-1}\left[2\left(q_0q_2-q_3q_1\right)\right]\\ \psi & = & \tan^{-1}\left[\frac{2\left(q_0q_3+q_1q_2\right)}{1-2\left(q_2^2+q_3^2\right)}\right]\\ \end{eqnarray*}

Parameters
[in]in_quatinput quaternion instance
Returns
converted euler angles
89{
90 euler out_euler;
91 if (!in_quat) // if null
92 {
93 fprintf(stderr, "%s: Invalid input.", __func__);
94 return out_euler;
95 }
96
97 out_euler.roll = atan2f(
98 2.f * (in_quat->w * in_quat->q1 + in_quat->q2 * in_quat->q3),
99 1.f - 2.f * (in_quat->q1 * in_quat->q1 + in_quat->q2 * in_quat->q2));
100 out_euler.pitch =
101 asinf(2.f * (in_quat->w * in_quat->q2 + in_quat->q1 * in_quat->q3));
102 out_euler.yaw = atan2f(
103 2.f * (in_quat->w * in_quat->q3 + in_quat->q1 * in_quat->q2),
104 1.f - 2.f * (in_quat->q2 * in_quat->q2 + in_quat->q3 * in_quat->q3));
105
106 return out_euler;
107}
3D Euler or Tait-Bryan angles (in radian)
Definition: geometry_datatypes.h:81
float roll
or bank = rotation about X axis
Definition: geometry_datatypes.h:84
float pitch
or elevation = rotation about Y axis
Definition: geometry_datatypes.h:89
float yaw
or heading = rotation about Z axis
Definition: geometry_datatypes.h:94
float w
real part of quaternion
Definition: geometry_datatypes.h:64

◆ quat_from_euler()

quaternion quat_from_euler ( const euler in_euler)

Function to convert given Euler angles to a quaternion.

\begin{eqnarray*} q_{0} & = &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\ q_{1} & = &\sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) - \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\ q_{2} & = &\cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\ q_{3} & = &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right) - \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)\\ \end{eqnarray*}

Parameters
[in]in_eulerinput Euler angles instance
Returns
converted quaternion
48{
49 quaternion out_quat;
50
51 if (!in_euler) // if null
52 {
53 fprintf(stderr, "%s: Invalid input.", __func__);
54 return out_quat;
55 }
56
57 quaternion temp;
58
59 float cy = cosf(in_euler->yaw * 0.5f);
60 float sy = sinf(in_euler->yaw * 0.5f);
61 float cp = cosf(in_euler->pitch * 0.5f);
62 float sp = sinf(in_euler->pitch * 0.5f);
63 float cr = cosf(in_euler->roll * 0.5f);
64 float sr = sinf(in_euler->roll * 0.5f);
65
66 temp.w = cr * cp * cy + sr * sp * sy;
67 temp.q1 = sr * cp * cy - cr * sp * sy;
68 temp.q2 = cr * sp * cy + sr * cp * sy;
69 temp.q3 = cr * cp * sy - sr * sp * cy;
70
71 return temp;
72}
a Quaternion type represented using a scalar or and a 3D vector
Definition: geometry_datatypes.h:61

◆ quaternion_multiply()

quaternion quaternion_multiply ( const quaternion in_quat1,
const quaternion in_quat2 
)

Function to multiply two quaternions.

\begin{eqnarray*} \mathbf{c} & = & \mathbf{a}\otimes\mathbf{b}\\ & = & \begin{bmatrix}a_{0} & a_{1} & a_{2} & a_{3}\end{bmatrix}\otimes\begin{bmatrix}b_{0} & b_{1} & b_{2} & b_{3}\end{bmatrix}\\ & = & \begin{bmatrix} a_{0}b_{0}-a_{1}b_{1}-a_{2}b_{2}-a_{3}b_{3}\\ a_{0}b_{1}+a_{1}b_{0}+a_{2}b_{3}-a_{3}b_{2}\\ a_{0}b_{2}-a_{1}b_{3}+a_{2}b_{0}+a_{3}b_{1}\\ a_{0}b_{3}+a_{1}b_{2}-a_{2}b_{1}+a_{3}b_{0} \end{bmatrix}^{T} \end{eqnarray*}

Parameters
[in]in_quat1first input quaternion instance
[in]in_quat2second input quaternion instance
Returns
resultant quaternion
131{
132 quaternion out_quat;
133 if (!in_quat1 || !in_quat2) // if null
134 {
135 fprintf(stderr, "%s: Invalid input.", __func__);
136 return out_quat;
137 }
138
139 out_quat.w = in_quat1->w * in_quat2->w - in_quat1->q1 * in_quat2->q1 -
140 in_quat1->q2 * in_quat2->q2 - in_quat1->q3 * in_quat2->q3;
141 out_quat.q1 = in_quat1->w * in_quat2->q1 + in_quat1->q1 * in_quat2->w +
142 in_quat1->q2 * in_quat2->q3 - in_quat1->q3 * in_quat2->q2;
143 out_quat.q2 = in_quat1->w * in_quat2->q2 - in_quat1->q1 * in_quat2->q3 +
144 in_quat1->q2 * in_quat2->w + in_quat1->q3 * in_quat2->q1;
145 out_quat.q3 = in_quat1->w * in_quat2->q3 + in_quat1->q1 * in_quat2->q2 -
146 in_quat1->q2 * in_quat2->q1 + in_quat1->q3 * in_quat2->w;
147
148 return out_quat;
149}