#include "attitude/quaternion.h"
#include "attitude/attitude_utils.h"
#include <math.h>
#include <stdio.h>

Functions
void	axis_angle_rotate (const double axis[3], double angle, const double v_in[3], double v_out[3])
	Rotate a vector about an axis by a given angle.

int	quaternion_inverse (const double q[4], double q_inv[4])
	Compute the inverse of a quaternion.

void	quaternion_multiply (const double q1[4], const double q2[4], double q_out[4])
	Multiply two quaternions using Hamilton product.

void	quaternion_normalize (double q[4])
	Normalise a quaternion in-place.

int	quaternion_orientation_error_axis_angle (const double q_current[4], const double q_target[4], double axis[3], double *angle)
	Compute current-to-target orientation error as an axis-angle command.

int	quaternion_relative (const double q_current[4], const double q_target[4], double q_error[4])
	Compute the relative rotation from the current orientation to a target orientation.

void	quaternion_rotate_vector (const double q[4], const double v_in[3], double v_out[3])
	Rotate a vector using a quaternion.

void	quaternion_rotate_vector_explicit (const double q[4], const double v_in[3], double v_out[3])
	Rotate a vector using the explicit \(q v q^\* \) formulation.

void	quaternion_set_explicit_debug (int enabled)
	Enable verbose logging for the explicit quaternion rotation implementation.

void	quaternion_slerp (const double q1[4], const double q2[4], double t, double q_out[4])
	Interpolate two quaternions using spherical linear interpolation (SLERP).

int	quaternion_to_axis_angle (const double q[4], double axis[3], double *angle)
	Convert a quaternion to axis-angle representation.

void	quaternion_to_dcm (const double q[4], double dcm[3][3])
	Convert a quaternion to a direction cosine matrix (DCM).

void	quaternion_to_euler (const double q[4], double roll, double pitch, double *yaw)
	Convert a quaternion to intrinsic ZYX Euler angles.

Function Documentation

◆ axis_angle_rotate()

void axis_angle_rotate	(	const double	axis[3],
		double	angle,
		const double	v_in[3],
		double	v_out[3]
	)

Rotate a vector about an axis by a given angle.

Parameters

axis	Unit rotation axis.
angle	Rotation angle in radians.
v_in	Input vector to rotate.
v_out	Output rotated vector.

Definition at line 198 of file quaternion.c.

                                                                                                  {
    double cos_theta = cos(angle);
    double sin_theta = sin(angle);
 
    double dot = axis[0] * v_in[0] + axis[1] * v_in[1] + axis[2] * v_in[2];
    double cross[3];
    cross[0] = axis[1] * v_in[2] - axis[2] * v_in[1];
    cross[1] = axis[2] * v_in[0] - axis[0] * v_in[2];
    cross[2] = axis[0] * v_in[1] - axis[1] * v_in[0];
 
    for (int i = 0; i < 3; ++i) {
        v_out[i] = v_in[i] * cos_theta + cross[i] * sin_theta + axis[i] * dot * (1 - cos_theta);
    }
}

◆ quaternion_inverse()

int quaternion_inverse	(	const double	q[4],
		double	q_inv[4]
	)

Compute the inverse of a quaternion.

The inverse is defined as \( q^{-1} = \frac{\bar{q}}{\|q\|^2} \), where \(\bar{q}\) is the conjugate.

Parameters

q	Input quaternion.
q_inv	Output inverse quaternion.

Returns: 1 if the inverse exists, 0 when the input norm is too small.

Definition at line 85 of file quaternion.c.

                                                           {
    double norm_sq = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
    if (norm_sq < 1e-14) {
        // Norm too close to zero
        return 0; // Indicate failure
    }
    q_inv[0] = q[0] / norm_sq;
    q_inv[1] = -q[1] / norm_sq;
    q_inv[2] = -q[2] / norm_sq;
    q_inv[3] = -q[3] / norm_sq;
    return 1; // Success
}

◆ quaternion_multiply()

void quaternion_multiply	(	const double	q1[4],
		const double	q2[4],
		double	q_out[4]
	)

Multiply two quaternions using Hamilton product.

Performs \( q_{\text{out}} = q_1 \otimes q_2 \) which corresponds to applying q2 first, followed by q1.

Parameters

q1	Left operand in \([w, x, y, z]\) order.
q2	Right operand in \([w, x, y, z]\) order.
q_out	Output quaternion product.

Definition at line 75 of file quaternion.c.

                                                                                  {
    double w1 = q1[0], x1 = q1[1], y1 = q1[2], z1 = q1[3];
    double w2 = q2[0], x2 = q2[1], y2 = q2[2], z2 = q2[3];
 
    q_out[0] = w1*w2 - x1*x2 - y1*y2 - z1*z2;
    q_out[1] = w1*x2 + x1*w2 + y1*z2 - z1*y2;
    q_out[2] = w1*y2 - x1*z2 + y1*w2 + z1*x2;
    q_out[3] = w1*z2 + x1*y2 - y1*x2 + z1*w2;
}

◆ quaternion_normalize()

void quaternion_normalize ( double q[4] )

Normalise a quaternion in-place.

If the quaternion norm is very small the vector is left unchanged.

Parameters

q	Quaternion to normalise.

Definition at line 57 of file quaternion.c.

                                       {
    double norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    
    // Only normalize if significantly different from 1.0
    if (norm > 1e-6 && fabs(norm - 1.0) > 1e-6) {
        q[0] /= norm;
        q[1] /= norm;
        q[2] /= norm;
        q[3] /= norm;
    }
    
    #ifdef DEBUG
    if (fabs(norm - 1.0) > 1e-6) {
        printf("Quaternion is not normalized: norm = %f\n", norm);
    }
    #endif
}

◆ quaternion_orientation_error_axis_angle()

int quaternion_orientation_error_axis_angle	(	const double	q_current[4],
		const double	q_target[4],
		double	axis[3],
		double *	angle
	)

Compute current-to-target orientation error as an axis-angle command.

This is useful for controller/debug flows where a camera tracker or planner provides a target attitude and the control loop needs a rotation axis plus angle magnitude.

Parameters

q_current	Current orientation quaternion.
q_target	Desired target orientation quaternion.
axis	Output unit correction axis.
angle	Output correction angle in radians.

Returns: 1 on success, 0 when the relative rotation cannot be computed.

Definition at line 117 of file quaternion.c.

                                                           {
    double q_error[4];
    if (!quaternion_relative(q_current, q_target, q_error)) {
        return 0;
    }
 
    return quaternion_to_axis_angle(q_error, axis, angle);
}

◆ quaternion_relative()

int quaternion_relative	(	const double	q_current[4],
		const double	q_target[4],
		double	q_error[4]
	)

Compute the relative rotation from the current orientation to a target orientation.

Produces \( q_{\text{error}} = q_{\text{target}} \otimes q_{\text{current}}^{-1} \). With this library's multiplication convention, applying q_error after q_current reconstructs q_target. The result is normalised and sign-adjusted so the scalar component is non-negative, giving the shortest equivalent rotation representation.

Parameters

q_current	Current orientation quaternion.
q_target	Desired target orientation quaternion.
q_error	Output relative rotation from current to target.

Returns: 1 on success, 0 when q_current cannot be inverted.

Definition at line 98 of file quaternion.c.

                                                                                                {
    double q_current_inv[4];
    if (!quaternion_inverse(q_current, q_current_inv)) {
        return 0;
    }
 
    quaternion_multiply(q_target, q_current_inv, q_error);
    quaternion_normalize(q_error);
 
    // q and -q represent the same rotation. Prefer the shortest positive-scalar form.
    if (q_error[0] < 0.0) {
        for (int i = 0; i < 4; ++i) {
            q_error[i] = -q_error[i];
        }
    }
 
    return 1;
}

◆ quaternion_rotate_vector()

void quaternion_rotate_vector	(	const double	q[4],
		const double	v_in[3],
		double	v_out[3]
	)

Rotate a vector using a quaternion.

Applies the efficient formulation described by \( v' = q v q^{-1} \) without explicitly forming the matrix.

Parameters

q	Rotation quaternion.
v_in	Input vector.
v_out	Output rotated vector.

Definition at line 213 of file quaternion.c.

                                                                                        {
    // Takes a quaternion q = [w,x,y,z], input vector v_in and stores result in v_out
    // q should be a unit quaternion (normalized)
    
    // Could do full quaternion multiplication q⊗v⊗q*
    // But this optimized formula is more efficient
    
    double q0q0 = q[0] * q[0];
    double q1q1 = q[1] * q[1];
    double q2q2 = q[2] * q[2];
    double q3q3 = q[3] * q[3];
    
    double q0q1 = q[0] * q[1];
    double q0q2 = q[0] * q[2];
    double q0q3 = q[0] * q[3];
    double q1q2 = q[1] * q[2];
    double q1q3 = q[1] * q[3];
    double q2q3 = q[2] * q[3];
    
    // Rotation matrix elements
    double r11 = q0q0 + q1q1 - q2q2 - q3q3;
    double r12 = 2.0 * (q1q2 - q0q3);
    double r13 = 2.0 * (q1q3 + q0q2);
    
    double r21 = 2.0 * (q1q2 + q0q3);
    double r22 = q0q0 - q1q1 + q2q2 - q3q3;
    double r23 = 2.0 * (q2q3 - q0q1);
    
    double r31 = 2.0 * (q1q3 - q0q2);
    double r32 = 2.0 * (q2q3 + q0q1);
    double r33 = q0q0 - q1q1 - q2q2 + q3q3;
    
    // Perform rotation
    v_out[0] = r11 * v_in[0] + r12 * v_in[1] + r13 * v_in[2];
    v_out[1] = r21 * v_in[0] + r22 * v_in[1] + r23 * v_in[2];
    v_out[2] = r31 * v_in[0] + r32 * v_in[1] + r33 * v_in[2];
}

◆ quaternion_rotate_vector_explicit()

void quaternion_rotate_vector_explicit	(	const double	q[4],
		const double	v_in[3],
		double	v_out[3]
	)

Rotate a vector using the explicit \(q v q^\* \) formulation.

Useful for educational tooling and verification of the optimised helper.

Parameters

q	Rotation quaternion.
v_in	Input vector.
v_out	Output rotated vector.

Definition at line 251 of file quaternion.c.

                                                                                                 {
    /* Educational expansion of q ⊗ v ⊗ q* with every intermediate exposed. */
 
    /* Step 1: Promote the vector into a "pure" quaternion (scalar part = 0). */
    double v_quat[4] = {0.0, v_in[0], v_in[1], v_in[2]};
    if (g_quaternion_explicit_debug) {
        printf("[explicit] Step 1 (vector→quat): [%.6f, %.6f, %.6f, %.6f]\n",
               v_quat[0], v_quat[1], v_quat[2], v_quat[3]);
    }
 
    /* Step 2: Compute the first product temp = q ⊗ v.
     *
     * Quaternion multiplication (a⊗b) is:
     *   w = aw*bw - ax*bx - ay*by - az*bz
     *   x = aw*bx + ax*bw + ay*bz - az*by
     *   y = aw*by - ax*bz + ay*bw + az*bx
     *   z = aw*bz + ax*by - ay*bx + az*bw
     *
     * With v's scalar component equal to zero the formulas collapse neatly. */
    const double w = q[0];
    const double x = q[1];
    const double y = q[2];
    const double z = q[3];
 
    const double vx = v_quat[1];
    const double vy = v_quat[2];
    const double vz = v_quat[3];
 
    double temp[4];
    temp[0] = -x * vx - y * vy - z * vz;
    temp[1] =  w * vx + y * vz - z * vy;
    temp[2] =  w * vy + z * vx - x * vz;
    temp[3] =  w * vz + x * vy - y * vx;
 
    if (g_quaternion_explicit_debug) {
        printf("[explicit] Step 2 (temp=q⊗v): [%.6f, %.6f, %.6f, %.6f]\n",
               temp[0], temp[1], temp[2], temp[3]);
    }
 
    /* Step 3: Form the conjugate q* = [w, -x, -y, -z].
     * For unit quaternions the conjugate equals the inverse. */
    const double q_conj[4] = {w, -x, -y, -z};
    if (g_quaternion_explicit_debug) {
        printf("[explicit] Step 3 (q_conj): [%.6f, %.6f, %.6f, %.6f]\n",
               q_conj[0], q_conj[1], q_conj[2], q_conj[3]);
    }
 
    /* Step 4: Multiply the intermediate quaternion by the conjugate:
     * result = temp ⊗ q*. */
    const double cw = q_conj[0];
    const double cx = q_conj[1];
    const double cy = q_conj[2];
    const double cz = q_conj[3];
 
    double result[4];
    result[0] = temp[0] * cw - temp[1] * cx - temp[2] * cy - temp[3] * cz;
    result[1] = temp[0] * cx + temp[1] * cw + temp[2] * cz - temp[3] * cy;
    result[2] = temp[0] * cy - temp[1] * cz + temp[2] * cw + temp[3] * cx;
    result[3] = temp[0] * cz + temp[1] * cy - temp[2] * cx + temp[3] * cw;
 
    if (g_quaternion_explicit_debug) {
        printf("[explicit] Step 4 (result=temp⊗q*): [%.6f, %.6f, %.6f, %.6f]\n",
               result[0], result[1], result[2], result[3]);
    }
 
    /* Step 5: The rotated vector is the imaginary part of the final quaternion. */
    v_out[0] = result[1];
    v_out[1] = result[2];
    v_out[2] = result[3];
    if (g_quaternion_explicit_debug) {
        printf("[explicit] Step 5 (rotated vector): [%.6f, %.6f, %.6f]\n",
               v_out[0], v_out[1], v_out[2]);
    }
}

◆ quaternion_set_explicit_debug()

void quaternion_set_explicit_debug ( int enabled )

Enable verbose logging for the explicit quaternion rotation implementation.

When enabled, the helper emits intermediate products which is useful for tutorials and debugging.

Parameters

enabled Non-zero value enables logging, zero disables it.

Definition at line 326 of file quaternion.c.

                                                {
    g_quaternion_explicit_debug = enabled;
}

◆ quaternion_slerp()

void quaternion_slerp	(	const double	q1[4],
		const double	q2[4],
		double	t,
		double	q_out[4]
	)

Interpolate two quaternions using spherical linear interpolation (SLERP).

Interpolates along the shortest unit-quaternion arc. The interpolation parameter is clamped to \([0, 1]\).

Parameters

q1	Start quaternion.
q2	End quaternion.
t	Interpolation parameter in \([0, 1]\).
q_out	Resulting interpolated quaternion.

Definition at line 146 of file quaternion.c.

                                                                                         {
    // Calculate dot product to determine the angle between quaternions
    double dot = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
 
    // Store the adjusted version of q2 to ensure shortest path rotation
    double q2_adjusted[4];
    
    // If dot product is negative, take the shorter path by negating q2
    // This is because q and -q represent the same rotation in quaternion space
    if (dot < 0.0) {
        dot = -dot;  // Make dot positive for subsequent calculations
        // Negate q2 to take shorter path
        q2_adjusted[0] = -q2[0];
        q2_adjusted[1] = -q2[1];
        q2_adjusted[2] = -q2[2];
        q2_adjusted[3] = -q2[3];
    } else {
        // If dot is positive, use q2 as is
        q2_adjusted[0] = q2[0];
        q2_adjusted[1] = q2[1];
        q2_adjusted[2] = q2[2];
        q2_adjusted[3] = q2[3];
    }
 
    // Optimization: If quaternions are very close (dot ≈ 1), use linear interpolation
    // This prevents numerical instability when dividing by sin(theta) for small angles
    if (dot > 0.9995) {
        // Perform linear interpolation (LERP)
        for (int i = 0; i < 4; ++i) {
            q_out[i] = q1[i] + t * (q2_adjusted[i] - q1[i]);
        }
        // Ensure the result is normalized to maintain unit quaternion property
        quaternion_normalize(q_out);
        return;
    }
 
    // Calculate the angle between quaternions
    double theta = acos(dot);
    double sin_theta = sin(theta);
 
    // Calculate interpolation weights
    // These weights ensure constant angular velocity
    double weight1 = sin((1 - t) * theta) / sin_theta;  // Weight for q1
    double weight2 = sin(t * theta) / sin_theta;        // Weight for q2
 
    // Perform the spherical interpolation
    // This creates a rotation that smoothly transitions from q1 to q2
    for (int i = 0; i < 4; ++i) {
        q_out[i] = weight1 * q1[i] + weight2 * q2_adjusted[i];
    }
}

◆ quaternion_to_axis_angle()

int quaternion_to_axis_angle	(	const double	q[4],
		double	axis[3],
		double *	angle
	)

Convert a quaternion to axis-angle representation.

Produces a unit rotation axis and right-handed rotation angle in radians.

Parameters

q	Input quaternion.
axis	Output unit axis vector.
angle	Output rotation magnitude (rad).

Returns: 1 on success, 0 when the axis is undefined (quaternion too close to identity).

Definition at line 129 of file quaternion.c.

                                                                               {
    double w = q[0];
    double norm_vec = sqrt(q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    if (norm_vec < 1e-14) {
        // Pure scalar quaternion (angle=0)
        *angle = 0.0;
        axis[0] = axis[1] = axis[2] = 0.0; // no rotation axis
        return 1; 
    }
    *angle = 2.0 * acos(w);
    double s = 1.0 / norm_vec;
    axis[0] = q[1]*s;
    axis[1] = q[2]*s;
    axis[2] = q[3]*s;
    return 1;
}

◆ quaternion_to_dcm()

void quaternion_to_dcm	(	const double	q[4],
		double	dcm[3][3]
	)

Convert a quaternion to a direction cosine matrix (DCM).

The resulting matrix rotates a vector from the body frame into the inertial frame: \( v_{\text{world}} = \mathbf{C}_{b}^{w} \, v_{\text{body}} \).

Parameters

q	Quaternion in \([w, x, y, z]\) order.
dcm	Output \(3\times3\) rotation matrix stored in row-major order.

Definition at line 8 of file quaternion.c.

                                                            {
    // q = [w, x, y, z]
    double w = q[0], x = q[1], y = q[2], z = q[3];
 
    double xx = x*x; double yy = y*y; double zz = z*z;
    double xy = x*y; double xz = x*z; double yz = y*z;
    double wx = w*x; double wy = w*y; double wz = w*z;
 
    dcm[0][0] = 1.0 - 2.0*(yy + zz);
    dcm[0][1] = 2.0*(xy - wz);
    dcm[0][2] = 2.0*(xz + wy);
    dcm[1][0] = 2.0*(xy + wz);
    dcm[1][1] = 1.0 - 2.0*(xx + zz);
    dcm[1][2] = 2.0*(yz - wx);
    dcm[2][0] = 2.0*(xz - wy);
    dcm[2][1] = 2.0*(yz + wx);
    dcm[2][2] = 1.0 - 2.0*(xx + yy);
}

◆ quaternion_to_euler()

void quaternion_to_euler	(	const double	q[4],
		double *	roll,
		double *	pitch,
		double *	yaw
	)

Convert a quaternion to intrinsic ZYX Euler angles.

Euler angles follow the aerospace convention (yaw→pitch→roll). Returned angles are in radians.

Parameters

q	Quaternion in \([w, x, y, z]\) order.
roll	Output rotation about the \(x\) axis (rad).
pitch	Output rotation about the \(y\) axis (rad).
yaw	Output rotation about the \(z\) axis (rad).

Definition at line 27 of file quaternion.c.

                                                                                      {
    double w = q[0], x = q[1], y = q[2], z = q[3];
 
    // Normalize quaternion
    double norm = sqrt(w * w + x * x + y * y + z * z);
    if (norm == 0.0) {
        *roll = 0.0;
        *pitch = 0.0;
        *yaw = 0.0;
        return;
    }
    w /= norm; x /= norm; y /= norm; z /= norm;
 
    // Compute intermediate values
    double sinr_cosp = 2.0 * (w * x + y * z);
    double cosr_cosp = 1.0 - 2.0 * (x * x + y * y);
    *roll = atan2(sinr_cosp, cosr_cosp);
 
    double sinp = 2.0 * (w * y - z * x);
    if (fabs(sinp) >= 1.0) {
        *pitch = copysign(ATTITUDE_PI / 2, sinp);
    } else {
        *pitch = asin(sinp);
    }
 
    double siny_cosp = 2.0 * (w * z + x * y);
    double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
    *yaw = atan2(siny_cosp, cosy_cosp);
}

Functions

Function Documentation

◆ axis_angle_rotate()

◆ quaternion_inverse()

◆ quaternion_multiply()

◆ quaternion_normalize()

◆ quaternion_orientation_error_axis_angle()

◆ quaternion_relative()

◆ quaternion_rotate_vector()

◆ quaternion_rotate_vector_explicit()

◆ quaternion_set_explicit_debug()

◆ quaternion_slerp()

◆ quaternion_to_axis_angle()

◆ quaternion_to_dcm()

◆ quaternion_to_euler()