gplib/include/gp/math.hpp

#pragma once
#include "gp_config.hpp"
#include "gp/math/integer_math.hpp"
#include "gp/math/q_math.hpp"
#if !defined(NO_FP_MATH)
#	include "gp/math/fp_math.hpp"
#endif
#include "gp/algorithm/repeat.hpp"

namespace gp {
	template<typename T, size_t fixed_passes = 16>
	T fixed_sqrt(T value) {
		if(value == 0) return 0;
		T ret = value / 2;
		T tmp;

		gp::repeat(fixed_passes, [&](){
			tmp = ret;
			ret = (value / tmp + tmp) / 2;
		});

		return ret;
	}

	template<typename T, size_t cap_passes = 16>
	T epsilon_sqrt(T value) {
		if(value == 0) return 0;
		T ret = value / 2;
		T tmp;
		constexpr T epsilon = gp_config::rendering::epsilon;
		size_t cnt = 0;
		while(
			!(
				(ret+epsilon)*ret > value
				&& (ret-epsilon)*ret < value
			)
			&& cnt < cap_passes
		){
			tmp = ret;
			ret = (value / tmp + tmp) / 2;
			++cnt;
		};

		return ret;
	}

	template<typename T, size_t cap_passes = 16>
	T stable_sqrt(T value) {
		if(value == 0) return 0;
		T ret = value / 2;
		T tmp;
		while(ret != tmp){
			tmp = ret;
			ret = (value / tmp + tmp) / 2;
		};

		return ret;
	}

	template<typename T = gp_config::rendering::default_type>
	struct vec2_g {
		T x;
		T y;

		vec2_g() 
		: x{}
		, y{}
		{}

		vec2_g(
			T _x,
			T _y
		) 
		: x{_x}
		, y{_y}
		{}

		vec2_g operator/(vec2_g rhs) {
			return {
				x / rhs.x,
				y / rhs.y
			};
		}

		vec2_g operator*(vec2_g rhs) {
			return {
				x * rhs.x,
				y * rhs.y
			};
		}

		vec2_g operator+(vec2_g oth) {
			return {x+oth.x, y+oth.y};
		}

		vec2_g operator-(vec2_g oth) {
			return {x-oth.x, y-oth.y};
		}

		vec2_g normalize() {
			T ilen = fast_isqrt(x*x+y*y);
			return {x*ilen, y*ilen};
		}
	};

	template<typename T = gp_config::rendering::default_type>
	struct vec3_g {
		T x;
		T y;
		T z;

		T& r(){
			return x;
		}
		T& g(){
			return y;
		}
		T& b(){
			return z;
		}

		vec3_g() 
		: x{}
		, y{}
		, z{}
		{}

		vec3_g(
			T _x,
			T _y,
			T _z
		) 
		: x{_x}
		, y{_y}
		, z{_z}
		{}

		vec3_g(vec2_g<T> left, T right) 
		: x{left.x}
		, y{left.y}
		, z{right}
		{}

		vec3_g(T left, vec2_g<T> right) 
		: x{left}
		, y{right.x}
		, z{right.y}
		{}

		vec3_g operator/(vec3_g rhs) {
			return {
				x / rhs.x,
				y / rhs.y,
				z / rhs.z
			};
		}

		vec3_g operator*(vec3_g rhs) {
			return {
				x * rhs.x,
				y * rhs.y,
				z * rhs.z
			};
		}

		vec3_g operator+(vec3_g oth) {
			return {x+oth.x, y+oth.y, z+oth.z};
		}

		vec3_g operator-(vec3_g oth) {
			return {x-oth.x, y-oth.y, z-oth.z};
		}

		vec3_g normalize() {
			T ilen = fast_isqrt(x*x+y*y+z*z);
			return {x*ilen, y*ilen, z*ilen};
		}
	};

	template<typename T = gp_config::rendering::default_type>
	struct vec4_g {
		T x;
		T y;
		T z;
		T w;

		T& r(){
			return x;
		}
		T& g(){
			return y;
		}
		T& b(){
			return z;
		}
		T& a(){
			return w;
		}

		vec4_g() 
		: x{}
		, y{}
		, z{}
		, w{}
		{}

		vec4_g(
			T _x,
			T _y,
			T _z,
			T _w
		) 
		: x{_x}
		, y{_y}
		, z{_z}
		, w{_w}
		{}

		vec4_g(T left, vec3_g<> right) 
		: x{left}
		, y{right.x}
		, z{right.y}
		, w{right.z}
		{}

		vec4_g(vec3_g<> left, T right) 
		: x{left.x}
		, y{left.y}
		, z{left.z}
		, w{right}
		{}

		vec4_g operator/(vec4_g rhs) {
			return {
				x / rhs.x,
				y / rhs.y,
				z / rhs.z,
				w / rhs.w
			};
		}

		vec4_g operator*(vec4_g rhs) {
			return {
				x * rhs.x,
				y * rhs.y,
				z * rhs.z,
				w * rhs.w
			};
		}

		vec4_g operator+(vec4_g oth) {
			return {x+oth.x, y+oth.y, z+oth.z, w+oth.w};
		}

		vec4_g operator-(vec4_g oth) {
			return {x-oth.x, y-oth.y, z-oth.w, z-oth.w};
		}

		vec4_g normalize() {
			T ilen = fast_isqrt(x*x+y*y+z*z+w*w);
			return {x*ilen, y*ilen, z*ilen, w*ilen};
		}
	};

	template<typename T>
	vec2_g<T> operator*(vec2_g<T> p, T v) {
		return {p.x*v, p.y*v};
	}

	template<typename T>
	vec3_g<T> operator*(vec3_g<T> p, T v) {
		return {p.x*v, p.y*v, p.z*v};
	}

	template<typename T>
	vec4_g<T> operator*(vec4_g<T> p, T v) {
		return {p.x*v, p.y*v, p.z*v, p.w*v};
	}

	template<typename T>
	vec2_g<T> operator*(T v, vec2_g<T> p) {
		return p*v;
	}

	template<typename T>
	vec3_g<T> operator*(T v, vec3_g<T> p) {
		return p*v;
	}

	template<typename T>
	vec4_g<T> operator*(T v, vec4_g<T> p) {
		return p*v;
	}
}

static_assert(sizeof(gp::vec3_g<int>) == sizeof(int)*3, "vec3_g has strange alignment");
static_assert(sizeof(gp::vec4_g<int>) == sizeof(int)*4, "vec4_g has strange alignment");