Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
574 lines (453 sloc) 19.8 KB
// 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;
constexpr T l = T::from_base(lhs.to_raw());
constexpr 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;
constexpr T l = T::from_base(lhs.to_raw());
constexpr 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;
constexpr T l = T::from_base(lhs.to_raw());
constexpr 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;
constexpr T l = T::from_base(lhs.to_raw());
constexpr 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