MathHelpers.hpp

#pragma once
 
// Standard.
#include <cmath>
 
// Custom.
#include "misc/Globals.h"
#include "io/Logger.h"
 
// External.
#include "math/GLMath.hpp"
#include "misc/Error.h"
#include "misc/Profiler.hpp"
 
namespace ne {
    class MathHelpers {
    public:
        MathHelpers() = delete;
 
        static inline glm::vec3 convertNormalizedDirectionToRollPitchYaw(const glm::vec3& direction);
 
        static inline glm::vec3 convertRollPitchYawToDirection(const glm::vec3& rotation);
 
        static inline glm::vec3 convertSphericalToCartesianCoordinates(float radius, float theta, float phi);
 
        static inline void convertCartesianCoordinatesToSpherical(
            const glm::vec3& location, float& radius, float& theta, float& phi);
 
        static inline glm::vec3 calculateReciprocalVector(const glm::vec3& vector);
 
        static inline glm::mat4x4 buildRotationMatrix(const glm::vec3& rotation);
 
        static inline float normalizeToRange(float value, float min, float max);
 
        static inline glm::vec3 normalizeSafely(const glm::vec3& vector);
 
    private:
        static inline const float smallFloatEpsilon = 0.0000001F; // NOLINT: not a very small number
    };
 
    glm::vec3 MathHelpers::convertNormalizedDirectionToRollPitchYaw(const glm::vec3& direction) {
        PROFILE_FUNC;
 
        // Ignore zero vectors.
        if (glm::all(glm::epsilonEqual(direction, glm::vec3(0.0F, 0.0F, 0.0F), smallFloatEpsilon))) {
            return glm::vec3(0.0F, 0.0F, 0.0F);
        }
 
#if defined(DEBUG)
        // Make sure we are given a normalized vector.
        constexpr float lengthDelta = 0.001F; // NOLINT: don't use too small value here
        const auto length = glm::length(direction);
        if (!glm::epsilonEqual(length, 1.0F, lengthDelta)) [[unlikely]] {
            // show an error so that it will be instantly noticeable because we're in the debug build
            Error error("the specified direction vector should have been normalized");
            error.showError();
            throw std::runtime_error(error.getFullErrorMessage());
        }
#endif
 
        glm::vec3 worldRotation = glm::vec3(0.0F, 0.0F, 0.0F);
 
        worldRotation.z = glm::degrees(std::atan2(direction.y, direction.x));
        worldRotation.y = glm::degrees(-std::asin(direction.z));
 
        // Check for NaNs.
        if (glm::isnan(worldRotation.z)) {
            Logger::get().warn(
                "found NaN in the Z component of the calculated rotation, setting this component's value to "
                "zero");
            worldRotation.z = 0.0F;
        }
        if (glm::isnan(worldRotation.y)) {
            Logger::get().warn(
                "found NaN in the Y component of the calculated rotation, setting this component's value to "
                "zero");
            worldRotation.y = 0.0F;
        }
 
        // Use zero roll for now.
 
        // Calculate roll:
        // See if we can use world up direction to find the right direction.
        // glm::vec3 vecToFindRight = worldUpDirection;
        // if (std::abs(direction.z) > 0.999F) { // NOLINT: magic number
        //    // Use +X then.
        //    vecToFindRight = glm::vec3(1.0F, 0.0F, 0.0F);
        //}
        // const auto rightDirection = glm::normalize(glm::cross(direction, vecToFindRight));
 
        // worldRotation.x =
        //     glm::degrees(-std::asinf(worldUpDirection.x * rightDirection.x + worldUpDirection.y *
        //     rightDirection.y));
 
        // Check roll for NaN.
        // if (glm::isnan(worldRotation.x)) {
        //     worldRotation.x = 0.0F;
        // }
 
        return worldRotation;
    }
 
    glm::vec3 MathHelpers::convertRollPitchYawToDirection(const glm::vec3& rotation) {
        return buildRotationMatrix(rotation) * glm::vec4(Globals::WorldDirection::forward, 0.0F);
    }
 
    glm::vec3 MathHelpers::convertSphericalToCartesianCoordinates(float radius, float theta, float phi) {
        phi = glm::radians(phi);
        theta = glm::radians(theta);
 
        const auto sinPhi = std::sin(phi);
        const auto sinTheta = std::sin(theta);
        const auto cosPhi = std::cos(phi);
        const auto cosTheta = std::cos(theta);
        return glm::vec3(radius * sinPhi * cosTheta, radius * sinPhi * sinTheta, radius * cosPhi);
    }
 
    void MathHelpers::convertCartesianCoordinatesToSpherical(
        const glm::vec3& location, float& radius, float& theta, float& phi) {
        radius = glm::sqrt(location.x * location.x + location.y * location.y + location.z * location.z);
        theta = glm::degrees(glm::atan2(location.y, location.x));
        phi = glm::degrees(
            glm::atan2(glm::sqrt(location.x * location.x + location.y * location.y), location.z));
    }
 
    glm::vec3 MathHelpers::calculateReciprocalVector(const glm::vec3& vector) {
        glm::vec3 reciprocal;
 
        if (std::abs(vector.x) < smallFloatEpsilon) [[unlikely]] {
            reciprocal.x = 0.0F;
        } else [[likely]] {
            reciprocal.x = 1.0F / vector.x;
        }
 
        if (std::abs(vector.y) < smallFloatEpsilon) [[unlikely]] {
            reciprocal.y = 0.0F;
        } else [[likely]] {
            reciprocal.y = 1.0F / vector.y;
        }
 
        if (std::abs(vector.z) < smallFloatEpsilon) [[unlikely]] {
            reciprocal.z = 0.0F;
        } else [[likely]] {
            reciprocal.z = 1.0F / vector.z;
        }
 
        return reciprocal;
    }
 
    glm::mat4x4 MathHelpers::buildRotationMatrix(const glm::vec3& rotation) {
        return glm::rotate(glm::radians(rotation.z), glm::vec3(0.0F, 0.0F, 1.0F)) *
               glm::rotate(glm::radians(rotation.y), glm::vec3(0.0F, 1.0F, 0.0F)) *
               glm::rotate(glm::radians(rotation.x), glm::vec3(1.0F, 0.0F, 0.0F));
    }
 
    float MathHelpers::normalizeToRange(float value, float min, float max) {
        const auto width = max - min;
        const auto offsetValue = value - min;
 
        return (offsetValue - (floor(offsetValue / width) * width)) + min;
    }
 
    glm::vec3 MathHelpers::normalizeSafely(const glm::vec3& vector) {
        const auto squareSum = vector.x * vector.x + vector.y * vector.y + vector.z * vector.z;
 
        if (squareSum < smallFloatEpsilon) {
            return glm::vec3(0.0F, 0.0F, 0.0F);
        }
 
        return vector * glm::inversesqrt(squareSum);
    }
} // namespace ne