gplib/include/gp/math/fp_math.hpp

#pragma once

#include "gp/algorithm/repeat.hpp"

#include <limits>

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

namespace gp{

	template<typename T>
	constexpr T pi;

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

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

	template<typename T>
	T abs(T);

	template<>
	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<>
	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<>
	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<>
	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;
		}
	}

	/**
	 * @brief Reads the sign of a value
	 * 
	 * @tparam T 
	 * @return -1 if the value is negative
	 * @return 0 if the value is 0 or not a number
	 * @return 1 if the value is positive
	 */
	template<typename T>
	T sign(T);

	template<>
	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<>
	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>
	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 
	 */
	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 
	 */
	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
	float cos(float v) {
		return sin(v+pi<float>/2);
	}

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

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

	// TODO: replace with an actual implementation
	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 
	 */
	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 
	 */
	double fast_isqrt(double v) {return isqrt<1>(v);}
}