Math.h

#pragma once
 
#include <glm/glm.hpp>
#include <glm/ext/quaternion_float.hpp>
 
#include <cmath>
 
namespace Cogs::Core
{
  inline glm::vec3 euclidean(const glm::vec4 h)
  {
    return (1.f / h.w) * glm::vec3(h);
  }
 
  inline glm::quat getRotation(glm::vec3 src, glm::vec3 dest)
  {
    glm::vec3 from = glm::normalize(src);
    glm::vec3 to = glm::normalize(dest);
 
    const float dot = glm::dot(from, to);
    auto crossvec = glm::cross(from, to);
    const float crosslen = glm::length(crossvec);
 
    if (crosslen == 0.0f) {
      if (dot > 0.0f) {
        return glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
      }
      else {
        glm::vec3 t = glm::cross(from, glm::vec3(1.0f, 0.0f, 0.0f));
 
        if (glm::length(t) == 0.0f) {
          t = glm::cross(from, glm::vec3(0.0f, 1.0f, 0.0f));
          t = glm::normalize(t);
        }
        return glm::quat(0.0f, t);
      }
    }
    else {
      crossvec = glm::normalize(crossvec);
 
      // Vectors are not parallel
      // The fabs() wrapping is to avoid problems when `dot' "overflows"
      // a tiny wee bit, which can lead to sqrt() returning NaN.
      crossvec *= std::sqrt(0.5f * std::fabs(1.0f - dot));
 
      // The fabs() wrapping is to avoid problems when `dot' "underflows"
      // a tiny wee bit, which can lead to sqrt() returning NaN.
      return glm::quat(std::sqrt(0.5f * std::fabs(1.0f + dot)), crossvec);
    }
  }
}