math.hpp

Go to the documentation of this file.

#ifndef GALAXIS_MATH_HPP
#define GALAXIS_MATH_HPP
#include <cmath>
 
namespace galaxis {
 
  namespace math {
    // Copied from: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
 
    struct Quaternion {
      double w = 0;
      double x = 0;
      double y = 0;
      double z = 0;
    };
 
    struct EulerAngles {
      double roll = 0;
      double pitch = 0;
      double yaw = 0;
    };
 
    Quaternion euler_angles_to_quaternion(EulerAngles const& e) { // yaw (Z), pitch (Y), roll (X)
 
      // Abbreviations for the various angular functions
      double cy = cos(e.yaw * 0.5);
      double sy = sin(e.yaw * 0.5);
      double cp = cos(e.pitch * 0.5);
      double sp = sin(e.pitch * 0.5);
      double cr = cos(e.roll * 0.5);
      double sr = sin(e.roll * 0.5);
 
      Quaternion q;
      q.w = cr * cp * cy + sr * sp * sy;
      q.x = sr * cp * cy - cr * sp * sy;
      q.y = cr * sp * cy + sr * cp * sy;
      q.z = cr * cp * sy - sr * sp * cy;
 
      return q;
    }
    
    EulerAngles quaternion_to_euler(Quaternion const& q) {
      EulerAngles angles;
 
      // roll (x-axis rotation)
      double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
      double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
      angles.roll = std::atan2(sinr_cosp, cosr_cosp);
 
      // pitch (y-axis rotation)
      double sinp = 2 * (q.w * q.y - q.z * q.x);
      if (std::abs(sinp) >= 1)
          angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
      else
          angles.pitch = std::asin(sinp);
 
      // yaw (z-axis rotation)
      double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
      double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
      angles.yaw = std::atan2(siny_cosp, cosy_cosp);
 
      return angles;
    }
    
  } // namespace math
 
} // namespace galaxis
 
#endif