4d_ObstacleRadarDroneCAN/math/axis_angle.c

#include "axis_angle.h"
#include "helpler_funtions.h"
#include "math.h"

void AxisAngle_ByQuaternion(const float Q[4], float axisAngle[3]) {
  float q_imag[3] = {Q[1], Q[2], Q[3]};
  float q_imag_mag = get_norm3(q_imag[0], q_imag[1], q_imag[2]);
  float r;
  if (q_imag_mag > 1e-10) {
    r = 2 * atan2f(q_imag_mag, Q[0]) / q_imag_mag;
  } else {
    r = 2 * sign(Q[0]);
  }

  for (int i = 0; i < 3; ++i) {
    axisAngle[i] = r * q_imag[i];
  }
}