quaternion.hpp

Go to the documentation of this file.

 
#ifndef INCLUDE_IMUNANO33_QUATERNION_HPP_
#define INCLUDE_IMUNANO33_QUATERNION_HPP_
 
#ifdef IMUNANO33_EMBED
#include <math.h>
#else
#include <cmath>
#endif
 
#ifdef IMUNANO33_EMBED
#include "imunano33/sv_embed.hpp"
#else
#include "imunano33/simplevectors.hpp"
#endif
#include "imunano33/unit.hpp"
 
namespace imunano33 {
#ifdef IMUNANO33_EMBED
using Vector3D =
    svector::EmbVec3D; 
#else
using std::cos;
using std::sin;
using std::sqrt;
using svector::Vector3D;
#endif
 
class Quaternion {
public:
  Quaternion() : m_w{1} {}
 
  Quaternion(const num_t w, const Vector3D &vec) : m_vec{vec} {
    // makes sure that quaternion magnitude is nonzero
    // this is important for rotations
    if (w == 0 && isZero(vec)) {
      m_w = 1;
    } else {
      m_w = w;
    }
  }
 
  Quaternion(const Vector3D &vec, const num_t ang) : m_w{cos(ang / 2)} {
    const Vector3D norm = normalize(vec);
    m_vec = normalize(vec) * sin(ang / 2);
  }
 
  Quaternion(const Quaternion &other) = default;
 
  Quaternion &operator=(const Quaternion &other) = default;
 
  ~Quaternion() = default;
 
  Quaternion(Quaternion &&) = default;
 
  Quaternion &operator=(Quaternion &&) = default;
 
  num_t w() const { return m_w; }
 
  Vector3D vec() const { return m_vec; }
 
  Quaternion conj() const { return Quaternion{m_w, -m_vec}; }
 
  Quaternion inv() const {
    const Quaternion conju = conj();
    const num_t mag = norm();
    const num_t newW = conju.w() / (mag * mag);
    const Vector3D newVec = conju.vec() / (mag * mag);
 
    return Quaternion{newW, newVec};
  }
 
  num_t norm() const {
    return sqrt(m_w * m_w + x(m_vec) * x(m_vec) + y(m_vec) * y(m_vec) +
                z(m_vec) * z(m_vec));
  }
 
  Quaternion unit() const {
    const num_t mag = norm();
    const num_t newW = m_w / mag;
    const Vector3D newVec = m_vec / mag;
 
    return Quaternion{newW, newVec};
  }
 
  // defined later, where operators are defined
  Quaternion &operator*=(const Quaternion &other);
  Vector3D rotate(const Vector3D &vec) const;
  static Vector3D rotate(const Vector3D &vec, const Vector3D &axis, num_t ang);
 
private:
  num_t m_w;
  Vector3D m_vec;
};
 
inline Quaternion operator*(const Quaternion &lhs, const Quaternion &rhs) {
#ifdef IMUNANO33_EMBED
  using Vector3D =
      svector::EmbVec3D; 
#else
  using svector::Vector3D;
#endif
 
  const num_t wl = lhs.w();
  const num_t wr = rhs.w();
 
  const Vector3D vl = lhs.vec();
  const Vector3D vr = rhs.vec();
 
  return Quaternion{wl * wr - dot(vl, vr), vr * wl + vl * wr + cross(vl, vr)};
}
 
inline bool operator==(const Quaternion &lhs, const Quaternion &rhs) {
  return lhs.w() == rhs.w() && lhs.vec() == rhs.vec();
}
 
inline bool operator!=(const Quaternion &lhs, const Quaternion &rhs) {
  return !(lhs == rhs);
}
 
inline Vector3D Quaternion::rotate(const Vector3D &vec, const Vector3D &axis,
                                   const num_t ang) {
  const Quaternion rotQ{normalize(axis), ang};
  const Quaternion vecQ{0, vec};
  const Quaternion res = rotQ * vecQ * rotQ.conj();
 
  return res.vec();
}
 
inline Quaternion &Quaternion::operator*=(const Quaternion &other) {
  const Quaternion res = (*this) * other;
  m_w = res.w();
  m_vec = res.vec();
 
  return *this;
}
 
inline Vector3D Quaternion::rotate(const Vector3D &vec) const {
  const Quaternion vecQ = {0, vec};
  const Quaternion res = (*this) * vecQ * inv();
 
  return res.vec();
}
} // namespace imunano33
 
#endif