TombEngine/Libs/fixedpoint/fixed_point.h

// From: https://github.com/eteran/cpp-utilities/blob/master/fixed/include/eteran/cpp-utilities/Fixed.h
// See also: http://stackoverflow.com/questions/79677/whats-the-best-way-to-do-fixed-point-math
/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2015 Evan Teran
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef FIXED_H_
#define FIXED_H_

#if __cplusplus >= 201402L
#define CONSTEXPR14 constexpr
#else
#define CONSTEXPR14
#endif


#include <ostream>
#include <exception>
#include <cstddef> // for size_t
#include <cstdint>
#include <type_traits>

namespace numeric {

	template <size_t I, size_t F>
	class Fixed;

	namespace detail {

		// helper templates to make magic with types :)
		// these allow us to determine resonable types from
		// a desired size, they also let us infer the next largest type
		// from a type which is nice for the division op
		template <size_t T>
		struct type_from_size {
			static constexpr bool is_specialized = false;
		};

#if defined(__GNUC__) && defined(__x86_64__) && !defined(__STRICT_ANSI__)
		template <>
		struct type_from_size<128> {
			static constexpr bool   is_specialized = true;
			static constexpr size_t size = 128;

			using value_type    = __int128;
			using unsigned_type = unsigned __int128;
			using signed_type   = __int128;
			using next_size     = type_from_size<256>;
		};
#endif

		template <>
		struct type_from_size<64> {
			static constexpr bool   is_specialized = true;
			static constexpr size_t size = 64;

			using value_type    = int64_t;
			using unsigned_type = std::make_unsigned<value_type>::type;
			using signed_type   = std::make_signed<value_type>::type;
			using next_size     = type_from_size<128>;
		};

		template <>
		struct type_from_size<32> {
			static constexpr bool   is_specialized = true;
			static constexpr size_t size = 32;

			using value_type    = int32_t;
			using unsigned_type = std::make_unsigned<value_type>::type;
			using signed_type   = std::make_signed<value_type>::type;
			using next_size     = type_from_size<64>;
		};

		template <>
		struct type_from_size<16> {
			static constexpr bool   is_specialized = true;
			static constexpr size_t size = 16;

			using value_type    = int16_t;
			using unsigned_type = std::make_unsigned<value_type>::type;
			using signed_type   = std::make_signed<value_type>::type;
			using next_size     = type_from_size<32>;
		};

		template <>
		struct type_from_size<8> {
			static constexpr bool   is_specialized = true;
			static constexpr size_t size = 8;

			using value_type    = int8_t;
			using unsigned_type = std::make_unsigned<value_type>::type;
			using signed_type   = std::make_signed<value_type>::type;
			using next_size     = type_from_size<16>;
		};

		// this is to assist in adding support for non-native base
		// types (for adding big-int support), this should be fine
		// unless your bit-int class doesn't nicely support casting
		template <class B, class N>
		constexpr B next_to_base(N rhs) {
			return static_cast<B>(rhs);
		}

		struct divide_by_zero : std::exception {
		};

		template <size_t I, size_t F>
		CONSTEXPR14 Fixed<I, F> divide(Fixed<I, F> numerator, Fixed<I, F> denominator, Fixed<I, F>& remainder, typename std::enable_if<type_from_size<I + F>::next_size::is_specialized>::type * = nullptr) {

			using next_type = typename Fixed<I, F>::next_type;
			using base_type = typename Fixed<I, F>::base_type;
			constexpr size_t fractional_bits = Fixed<I, F>::fractional_bits;

			next_type t(numerator.to_raw());
			t <<= fractional_bits;

			Fixed<I, F> quotient;

			quotient = Fixed<I, F>::from_base(next_to_base<base_type>(t / denominator.to_raw()));
			remainder = Fixed<I, F>::from_base(next_to_base<base_type>(t % denominator.to_raw()));

			return quotient;
		}

		template <size_t I, size_t F>
		CONSTEXPR14 Fixed<I, F> divide(Fixed<I, F> numerator, Fixed<I, F> denominator, Fixed<I, F> & remainder, typename std::enable_if<!type_from_size<I + F>::next_size::is_specialized>::type * = nullptr) {

			// NOTE(eteran): division is broken for large types :-(
			// especially when dealing with negative quantities

			using base_type     = typename Fixed<I, F>::base_type;
			using unsigned_type = typename Fixed<I, F>::unsigned_type;

			constexpr int bits = Fixed<I, F>::total_bits;

			if (denominator == 0) {
				throw divide_by_zero();
			}
			else {

				int sign = 0;

				Fixed<I, F> quotient;

				if (numerator < 0) {
					sign ^= 1;
					numerator = -numerator;
				}

				if (denominator < 0) {
					sign ^= 1;
					denominator = -denominator;
				}

				base_type n = numerator.to_raw();
				base_type d = denominator.to_raw();
				base_type x = 1;
				base_type answer = 0;

				// egyptian division algorithm
				while ((n >= d) && (((d >> (bits - 1)) & 1) == 0)) {
					x <<= 1;
					d <<= 1;
				}

				while (x != 0) {
					if (n >= d) {
						n -= d;
						answer += x;
					}

					x >>= 1;
					d >>= 1;
				}

				unsigned_type l1 = n;
				unsigned_type l2 = denominator.to_raw();

				// calculate the lower bits (needs to be unsigned)
				// unfortunately for many fractions this overflows the type still :-/
				const unsigned_type lo = (static_cast<unsigned_type>(n) << F) / denominator.to_raw();

				quotient = Fixed<I, F>::from_base((answer << F) | lo);
				remainder = n;

				if (sign) {
					quotient = -quotient;
				}

				return quotient;
			}
		}

		// this is the usual implementation of multiplication
		template <size_t I, size_t F>
		CONSTEXPR14 Fixed<I, F> multiply(Fixed<I, F> lhs, Fixed<I, F> rhs, typename std::enable_if<type_from_size<I + F>::next_size::is_specialized>::type * = nullptr) {

			using next_type = typename Fixed<I, F>::next_type;
			using base_type = typename Fixed<I, F>::base_type;

			constexpr size_t fractional_bits = Fixed<I, F>::fractional_bits;

			next_type t(static_cast<next_type>(lhs.to_raw()) * static_cast<next_type>(rhs.to_raw()));
			t >>= fractional_bits;

			return Fixed<I, F>::from_base(next_to_base<base_type>(t));
		}

		// this is the fall back version we use when we don't have a next size
		// it is slightly slower, but is more robust since it doesn't
		// require and upgraded type
		template <size_t I, size_t F>
		CONSTEXPR14 Fixed<I, F> multiply(Fixed<I, F> lhs, Fixed<I, F> rhs, typename std::enable_if<!type_from_size<I + F>::next_size::is_specialized>::type * = nullptr) {

			using base_type = typename Fixed<I, F>::base_type;

			constexpr size_t fractional_bits = Fixed<I, F>::fractional_bits;
			constexpr base_type integer_mask = Fixed<I, F>::integer_mask;
			constexpr base_type fractional_mask = Fixed<I, F>::fractional_mask;

			// more costly but doesn't need a larger type
			constexpr base_type a_hi = (lhs.to_raw() & integer_mask) >> fractional_bits;
			constexpr base_type b_hi = (rhs.to_raw() & integer_mask) >> fractional_bits;
			constexpr base_type a_lo = (lhs.to_raw() & fractional_mask);
			constexpr base_type b_lo = (rhs.to_raw() & fractional_mask);

			constexpr base_type x1 = a_hi * b_hi;
			constexpr base_type x2 = a_hi * b_lo;
			constexpr base_type x3 = a_lo * b_hi;
			constexpr base_type x4 = a_lo * b_lo;

			return Fixed<I, F>::from_base((x1 << fractional_bits) + (x3 + x2) + (x4 >> fractional_bits));
		}
	}

	template <size_t I, size_t F>
	class Fixed {
		static_assert(detail::type_from_size<I + F>::is_specialized, "invalid combination of sizes");

	public:
		static constexpr size_t fractional_bits = F;
		static constexpr size_t integer_bits = I;
		static constexpr size_t total_bits = I + F;

		using base_type_info = detail::type_from_size<total_bits>;

		using base_type     = typename base_type_info::value_type;
		using next_type     = typename base_type_info::next_size::value_type;
		using unsigned_type = typename base_type_info::unsigned_type;

	public:
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Woverflow"
#endif
		static constexpr base_type fractional_mask = ~(static_cast<unsigned_type>(~base_type(0)) << fractional_bits);
		static constexpr base_type integer_mask = ~fractional_mask;
#ifdef __GNUC__
#pragma GCC diagnostic push
#endif

	public:
		static constexpr base_type one = base_type(1) << fractional_bits;

	public: // constructors
		Fixed() = default;
		Fixed(const Fixed&) = default;
		Fixed & operator=(const Fixed&) = default;

		template <class Number>
		constexpr Fixed(Number n, typename std::enable_if<std::is_arithmetic<Number>::value>::type * = nullptr) : data_(static_cast<base_type>(n * one)) {
		}

	public: // conversion
		template <size_t I2, size_t F2>
		CONSTEXPR14 explicit Fixed(Fixed<I2, F2> other) {
			static_assert(I2 <= I && F2 <= F, "Scaling conversion can only upgrade types");
			using T = Fixed<I2, F2>;

			const base_type fractional = (other.data_ & T::fractional_mask);
			const base_type integer = (other.data_ & T::integer_mask) >> T::fractional_bits;
			data_ = (integer << fractional_bits) | (fractional << (fractional_bits - T::fractional_bits));
		}

	private:
		// this makes it simpler to create a fixed point object from
		// a native type without scaling
		// use "Fixed::from_base" in order to perform this.
		struct NoScale {};

		constexpr Fixed(base_type n, const NoScale&) : data_(n) {
		}

	public:
		constexpr static Fixed from_base(base_type n) {
			return Fixed(n, NoScale());
		}

	public:	// comparison operators
		constexpr bool operator==(Fixed rhs) const {
			return data_ == rhs.data_;
		}

		constexpr bool operator!=(Fixed rhs) const {
			return data_ != rhs.data_;
		}

		constexpr bool operator<(Fixed rhs) const {
			return data_ < rhs.data_;
		}

		constexpr bool operator>(Fixed rhs) const {
			return data_ > rhs.data_;
		}

		constexpr bool operator<=(Fixed rhs) const {
			return data_ <= rhs.data_;
		}

		constexpr bool operator>=(Fixed rhs) const {
			return data_ >= rhs.data_;
		}

	public:	// unary operators
		constexpr bool operator!() const {
			return !data_;
		}

		constexpr Fixed operator~() const {
			// NOTE(eteran): this will often appear to "just negate" the value
			// that is not an error, it is because -x == (~x+1)
			// and that "+1" is adding an infinitesimally small fraction to the
			// complimented value
			return Fixed::from_base(~data_);
		}

		constexpr Fixed operator-() const {
			return Fixed::from_base(-data_);
		}

		constexpr Fixed operator+() const {
			return Fixed::from_base(+data_);
		}

		CONSTEXPR14 Fixed& operator++() {
			data_ += one;
			return *this;
		}

		CONSTEXPR14 Fixed& operator--() {
			data_ -= one;
			return *this;
		}

		CONSTEXPR14 Fixed operator++(int) {
			Fixed tmp(*this);
			data_ += one;
			return tmp;
		}

		CONSTEXPR14 Fixed operator--(int) {
			Fixed tmp(*this);
			data_ -= one;
			return tmp;
		}

	public:	// basic math operators
		CONSTEXPR14 Fixed& operator+=(Fixed n) {
			data_ += n.data_;
			return *this;
		}

		CONSTEXPR14 Fixed& operator-=(Fixed n) {
			data_ -= n.data_;
			return *this;
		}

		CONSTEXPR14 Fixed& operator*=(Fixed n) {
			return assign(detail::multiply(*this, n));
		}

		CONSTEXPR14 Fixed& operator/=(Fixed n) {
			Fixed temp;
			return assign(detail::divide(*this, n, temp));
		}

	private:
		CONSTEXPR14 Fixed& assign(Fixed rhs) {
			data_ = rhs.data_;
			return *this;
		}

	public: // binary math operators, effects underlying bit pattern since these 
			// don't really typically make sense for non-integer values
		CONSTEXPR14 Fixed& operator&=(Fixed n) {
			data_ &= n.data_;
			return *this;
		}

		CONSTEXPR14 Fixed& operator|=(Fixed n) {
			data_ |= n.data_;
			return *this;
		}

		CONSTEXPR14 Fixed& operator^=(Fixed n) {
			data_ ^= n.data_;
			return *this;
		}

		template <class Integer, class = typename std::enable_if<std::is_integral<Integer>::value>::type>
		CONSTEXPR14 Fixed & operator>>=(Integer n) {
			data_ >>= n;
			return *this;
		}

		template <class Integer, class = typename std::enable_if<std::is_integral<Integer>::value>::type>
		CONSTEXPR14 Fixed & operator<<=(Integer n) {
			data_ <<= n;
			return *this;
		}

	public: // conversion to basic types
		constexpr int to_int() const {
			return (data_ & integer_mask) >> fractional_bits;
		}

		constexpr unsigned int to_uint() const {
			return (data_ & integer_mask) >> fractional_bits;
		}

		constexpr float to_float() const {
			return static_cast<float>(data_) / Fixed::one;
		}

		constexpr double to_double() const {
			return static_cast<double>(data_) / Fixed::one;
		}

		constexpr base_type to_raw() const {
			return data_;
		}

	public:
		CONSTEXPR14 void swap(Fixed & rhs) {
			using std::swap;
			swap(data_, rhs.data_);
		}

	public:
		base_type data_ = 0;
	};

	// if we have the same fractional portion, but differing integer portions, we trivially upgrade the smaller type
	template <size_t I1, size_t I2, size_t F>
	CONSTEXPR14 typename std::conditional<I1 >= I2, Fixed<I1, F>, Fixed<I2, F>>::type operator+(Fixed<I1, F> lhs, Fixed<I2, F> rhs) {

		using T = typename std::conditional<
			I1 >= I2,
			Fixed<I1, F>,
			Fixed<I2, F>
		>::type;

		const T l = T::from_base(lhs.to_raw());
		const T r = T::from_base(rhs.to_raw());
		return l + r;
	}

	template <size_t I1, size_t I2, size_t F>
	CONSTEXPR14 typename std::conditional<I1 >= I2, Fixed<I1, F>, Fixed<I2, F>>::type operator-(Fixed<I1, F> lhs, Fixed<I2, F> rhs) {

		using T = typename std::conditional<
			I1 >= I2,
			Fixed<I1, F>,
			Fixed<I2, F>
		>::type;

		const T l = T::from_base(lhs.to_raw());
		const T r = T::from_base(rhs.to_raw());
		return l - r;
	}

	template <size_t I1, size_t I2, size_t F>
	CONSTEXPR14 typename std::conditional<I1 >= I2, Fixed<I1, F>, Fixed<I2, F>>::type operator*(Fixed<I1, F> lhs, Fixed<I2, F> rhs) {

		using T = typename std::conditional<
			I1 >= I2,
			Fixed<I1, F>,
			Fixed<I2, F>
		>::type;

		const T l = T::from_base(lhs.to_raw());
		const T r = T::from_base(rhs.to_raw());
		return l * r;
	}

	template <size_t I1, size_t I2, size_t F>
	CONSTEXPR14 typename std::conditional<I1 >= I2, Fixed<I1, F>, Fixed<I2, F>>::type operator/(Fixed<I1, F> lhs, Fixed<I2, F> rhs) {

		using T = typename std::conditional<
			I1 >= I2,
			Fixed<I1, F>,
			Fixed<I2, F>
		>::type;

		const T l = T::from_base(lhs.to_raw());
		const T r = T::from_base(rhs.to_raw());
		return l / r;
	}

	template <size_t I, size_t F>
	std::ostream& operator<<(std::ostream & os, Fixed<I, F> f) {
		os << f.to_double();
		return os;
	}

	// basic math operators
	template <size_t I, size_t F> CONSTEXPR14 Fixed<I, F> operator+(Fixed<I, F> lhs, Fixed<I, F> rhs) { lhs += rhs; return lhs; }
	template <size_t I, size_t F> CONSTEXPR14 Fixed<I, F> operator-(Fixed<I, F> lhs, Fixed<I, F> rhs) { lhs -= rhs; return lhs; }
	template <size_t I, size_t F> CONSTEXPR14 Fixed<I, F> operator*(Fixed<I, F> lhs, Fixed<I, F> rhs) { lhs *= rhs; return lhs; }
	template <size_t I, size_t F> CONSTEXPR14 Fixed<I, F> operator/(Fixed<I, F> lhs, Fixed<I, F> rhs) { lhs /= rhs; return lhs; }

	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> CONSTEXPR14 Fixed<I, F> operator+(Fixed<I, F> lhs, Number rhs) { lhs += Fixed<I, F>(rhs); return lhs; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> CONSTEXPR14 Fixed<I, F> operator-(Fixed<I, F> lhs, Number rhs) { lhs -= Fixed<I, F>(rhs); return lhs; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> CONSTEXPR14 Fixed<I, F> operator*(Fixed<I, F> lhs, Number rhs) { lhs *= Fixed<I, F>(rhs); return lhs; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> CONSTEXPR14 Fixed<I, F> operator/(Fixed<I, F> lhs, Number rhs) { lhs /= Fixed<I, F>(rhs); return lhs; }

	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> CONSTEXPR14 Fixed<I, F> operator+(Number lhs, Fixed<I, F> rhs) { Fixed<I, F> tmp(lhs); tmp += rhs; return tmp; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> CONSTEXPR14 Fixed<I, F> operator-(Number lhs, Fixed<I, F> rhs) { Fixed<I, F> tmp(lhs); tmp -= rhs; return tmp; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> CONSTEXPR14 Fixed<I, F> operator*(Number lhs, Fixed<I, F> rhs) { Fixed<I, F> tmp(lhs); tmp *= rhs; return tmp; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> CONSTEXPR14 Fixed<I, F> operator/(Number lhs, Fixed<I, F> rhs) { Fixed<I, F> tmp(lhs); tmp /= rhs; return tmp; }

	// shift operators
	template <size_t I, size_t F, class Integer, class = typename std::enable_if<std::is_integral<Integer>::value>::type> CONSTEXPR14 Fixed<I, F> operator<<(Fixed<I, F> lhs, Integer rhs) { lhs <<= rhs; return lhs; }
	template <size_t I, size_t F, class Integer, class = typename std::enable_if<std::is_integral<Integer>::value>::type> CONSTEXPR14 Fixed<I, F> operator>>(Fixed<I, F> lhs, Integer rhs) { lhs >>= rhs; return lhs; }

	// comparison operators
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator>(Fixed<I, F> lhs, Number rhs) { return lhs > Fixed<I, F>(rhs); }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator<(Fixed<I, F> lhs, Number rhs) { return lhs < Fixed<I, F>(rhs); }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator>=(Fixed<I, F> lhs, Number rhs) { return lhs >= Fixed<I, F>(rhs); }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator<=(Fixed<I, F> lhs, Number rhs) { return lhs <= Fixed<I, F>(rhs); }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator==(Fixed<I, F> lhs, Number rhs) { return lhs == Fixed<I, F>(rhs); }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator!=(Fixed<I, F> lhs, Number rhs) { return lhs != Fixed<I, F>(rhs); }

	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator>(Number lhs, Fixed<I, F> rhs) { return Fixed<I, F>(lhs) > rhs; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator<(Number lhs, Fixed<I, F> rhs) { return Fixed<I, F>(lhs) < rhs; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator>=(Number lhs, Fixed<I, F> rhs) { return Fixed<I, F>(lhs) >= rhs; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator<=(Number lhs, Fixed<I, F> rhs) { return Fixed<I, F>(lhs) <= rhs; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator==(Number lhs, Fixed<I, F> rhs) { return Fixed<I, F>(lhs) == rhs; }
	template <size_t I, size_t F, class Number, class = typename std::enable_if<std::is_arithmetic<Number>::value>::type> constexpr bool operator!=(Number lhs, Fixed<I, F> rhs) { return Fixed<I, F>(lhs) != rhs; }
}

#undef CONSTEXPR14

#endif