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.
euler	euler_from_quat (const quaternion *in_quat)
	Function to convert given quaternion to Euler angles.
quaternion	quaternion_multiply (const quaternion *in_quat1, const quaternion *in_quat2)
	Function to multiply two quaternions.

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_quat

input quaternion instance

Returns

converted euler angles

{
    euler out_euler;
    if (!in_quat)  // if null
    {
        fprintf(stderr, "%s: Invalid input.", __func__);
        return out_euler;
    }
 
    out_euler.roll = atan2f(
        2.f * (in_quat->w * in_quat->q1 + in_quat->q2 * in_quat->q3),
        1.f - 2.f * (in_quat->q1 * in_quat->q1 + in_quat->q2 * in_quat->q2));
    out_euler.pitch =
        asinf(2.f * (in_quat->w * in_quat->q2 + in_quat->q1 * in_quat->q3));
    out_euler.yaw = atan2f(
        2.f * (in_quat->w * in_quat->q3 + in_quat->q1 * in_quat->q2),
        1.f - 2.f * (in_quat->q2 * in_quat->q2 + in_quat->q3 * in_quat->q3));
 
    return out_euler;
}

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_euler

input Euler angles instance

Returns

converted quaternion

{
    quaternion out_quat;
 
    if (!in_euler)  // if null
    {
        fprintf(stderr, "%s: Invalid input.", __func__);
        return out_quat;
    }
 
    quaternion temp;
 
    float cy = cosf(in_euler->yaw * 0.5f);
    float sy = sinf(in_euler->yaw * 0.5f);
    float cp = cosf(in_euler->pitch * 0.5f);
    float sp = sinf(in_euler->pitch * 0.5f);
    float cr = cosf(in_euler->roll * 0.5f);
    float sr = sinf(in_euler->roll * 0.5f);
 
    temp.w = cr * cp * cy + sr * sp * sy;
    temp.q1 = sr * cp * cy - cr * sp * sy;
    temp.q2 = cr * sp * cy + sr * cp * sy;
    temp.q3 = cr * cp * sy - sr * sp * cy;
 
    return temp;
}

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_quat1	first input quaternion instance
[in]	in_quat2	second input quaternion instance

Returns

resultant quaternion

{
    quaternion out_quat;
    if (!in_quat1 || !in_quat2)  // if null
    {
        fprintf(stderr, "%s: Invalid input.", __func__);
        return out_quat;
    }
 
    out_quat.w = in_quat1->w * in_quat2->w - in_quat1->q1 * in_quat2->q1 -
                 in_quat1->q2 * in_quat2->q2 - in_quat1->q3 * in_quat2->q3;
    out_quat.q1 = in_quat1->w * in_quat2->q1 + in_quat1->q1 * in_quat2->w +
                  in_quat1->q2 * in_quat2->q3 - in_quat1->q3 * in_quat2->q2;
    out_quat.q2 = in_quat1->w * in_quat2->q2 - in_quat1->q1 * in_quat2->q3 +
                  in_quat1->q2 * in_quat2->w + in_quat1->q3 * in_quat2->q1;
    out_quat.q3 = in_quat1->w * in_quat2->q3 + in_quat1->q1 * in_quat2->q2 -
                  in_quat1->q2 * in_quat2->q1 + in_quat1->q3 * in_quat2->w;
 
    return out_quat;
}