gplib/include/gp/math/ieee754/fp_math.hpp

#pragma once


#include "gp/algorithms/repeat.hpp"
#include "gp/math/details/math_definitions.hpp"

#include <limits>

#include <stddef.h>
#include <stdint.h>

namespace gp{
	namespace math{

		template<>
		constexpr float pi<float> = 3.1415926535897932384626433832795028841971693993751058209749445923078164062;

		template<>
		constexpr double pi<double> = 3.1415926535897932384626433832795028841971693993751058209749445923078164062L;


		template<>
		constexpr float abs<float>(float value) {
			static_assert(sizeof(float) == 4, "bad float size");
			union {
				float fp;
				uint32_t ab;
			} p;
			p.fp = value;
			p.ab &= 0x7fFFffFF;
			return p.fp;
		}

		template<>
		constexpr double abs<double>(double value) {
			static_assert(sizeof(double) == 8, "bad double size");
			union {
				double fp;
				uint64_t ab;
			} p;
			p.fp = value;
			p.ab &= 0x7fFFffFFffFFffFF;
			return p.fp;
		}

		template<typename T>
		T floor(T);

		template<>
		constexpr float floor<float>(float value) {
			static_assert(sizeof(float) == 4, "bad float size");
			if(
				value >= 16777216
				|| value <= std::numeric_limits<int32_t>::min() 
				|| value != value
			) {
				return value;
			}
			int32_t ret = value;
			float ret_d = ret;
			if(value == ret_d || value >= 0) {
				return ret;
			} else {
				return ret-1;
			}
		}

		template<>
		constexpr double floor<double>(double value) {
			static_assert(sizeof(double) == 8, "bad double size");
			if(
				value >= 9007199254740992
				|| value <= std::numeric_limits<int64_t>::min() 
				|| value != value
			) {
				return value;
			}
			int64_t ret = value;
			double ret_d = ret;
			if(value == ret_d || value >= 0) {
				return ret;
			} else {
				return ret-1;
			}
		}

		template<>
		constexpr float sign<float>(float value) {
			static_assert(sizeof(float) == 4, "bad float size");
			if(!value) return 0;
			union {
				float fp;
				uint32_t ab;
			} p;
			p.fp = value;
			p.ab &= 0x7fFFffFF;
			return value/p.fp;
		}

		template<>
		constexpr double sign<double>(double value) {
			static_assert(sizeof(double) == 8, "bad double size");
			if(!value) return 0;
			union {
				double fp;
				uint64_t ab;
			} p;
			p.fp = value;
			p.ab &= 0x7fFFffFFffFFffFF;
			return value/p.fp;
		}


		/**
		* @brief Calculate the sin of a value using Taylor's method
		* 
		* @tparam steps The number of steps to do at the maximum
		* @tparam T the type of value and the return type expected
		* @tparam accuracy the maximum accuracy to shoot for (early stopping)
		* @param value The value to calculate the sin of. Works better for values close to 0.
		* @return T the sin of the value (the sign may be off idk I don't remember)
		*/
		template<size_t steps, typename T, size_t accuracy = 1000000>
		constexpr T sin_taylor(T value) {
			const T acc = T{1}/T{accuracy};
			T B = value;
			T C = 1;
			T ret = B/C;
			for(size_t i = 1; (i < steps) && (abs<>(B/C) > acc); ++i) {
				B *= -1*value*value;
				C *= 2*i*(2*i+1);
				ret += B/C;
			}
			return ret;
		}

		/**
		* @brief General purpose sin function
		* 
		* @param v 
		* @return float 
		*/
		constexpr inline float sin(float v) {
			// limit the range between -pi and +pi
			v += pi<float>;
			v = v - 2*pi<float>*floor(v/(2*pi<float>));
			v -= pi<float>;
			float s = sign(v);
			v *= s;

			// use taylor's method on the value
			return sin_taylor<10>(v)*s;
		}

		/**
		* @brief General purpose sin function
		* 
		* @param v 
		* @return double 
		*/
		constexpr inline double sin(double v) {
			v += pi<double>;
			v = v - 2*pi<double>*floor(v/(2*pi<double>));
			v -= pi<double>;
			float s = sign(v);
			v *= s;
			return sin_taylor<10>(v)*s;
		}


		// TODO: replace with an actual implementation
		constexpr inline float cos(float v) {
			return sin(v+pi<float>/2);
		}

		// TODO: replace with an actual implementation
		constexpr inline double cos(double v) {
			return sin(v+pi<double>/2);
		}

		// TODO: replace with an actual implementation
		constexpr inline float tan(float v) {
			return sin(v)/cos(v);
		}

		// TODO: replace with an actual implementation
		constexpr inline double tan(double v) {
			return sin(v)/cos(v);
		}


		/**
		* @brief Quake isqrt (x) -> 1/sqrt(x)
		* 
		* @tparam cycles the number of newton method cycles to apply
		* @param v the value to apply the function on
		* @return float \f$\frac{1}{\sqrt{v}}\f$
		*/
		template<size_t cycles = 5>
		float isqrt(float v) {
			int32_t i;
			float x2, y;
			constexpr float threehalfs = 1.5F;

			x2 = v * 0.5F;
			y  = v;
			i  = * ( int32_t * ) &y;
			i  = 0x5F375A86 - ( i >> 1 );
			y  = * ( float * ) &i;
			gp::repeat(cycles, [&](){
				y  = y * ( threehalfs - ( x2 * y * y ) );
			});
			return y;
		}

		/**
		* @brief Quake isqrt (x) -> 1/sqrt(x) but for doubles
		* 
		* @tparam cycles the number of newton method cycles to apply
		* @param v the value to apply the function on
		* @return double \f$\frac{1}{\sqrt{v}}\f$
		*/
		template<size_t cycles = 5>
		double isqrt(double v) {
			int64_t i;
			double x2, y;
			constexpr double threehalfs = 1.5F;

			x2 = v * 0.5F;
			y  = v;
			i  = * ( int64_t * ) &y;
			i  = 0x5FE6EB50C7B537A9 - ( i >> 1 );
			y  = * ( double * ) &i;
			gp::repeat(cycles, [&](){
				y  = y * ( threehalfs - ( x2 * y * y ) );
			});
			return y;
		}

		/**
		* @brief A faster version of the Quake isqrt (actually the same but with defined cycles)
		* 
		* @param v 
		* @return float 
		*/
		inline float fast_isqrt(float v) {return isqrt<1>(v);}
		/**
		* @brief A faster version of the Quake isqrt (actually the same but with defined cycles)
		* 
		* @param v 
		* @return double 
		*/
		inline double fast_isqrt(double v) {return isqrt<1>(v);}
	}
}