diff options
| -rw-r--r-- | src/openvic-simulation/types/fixed_point/FixedPoint.hpp | 6 | ||||
| -rw-r--r-- | src/openvic-simulation/utility/NumberUtils.hpp | 29 |
2 files changed, 33 insertions, 2 deletions
diff --git a/src/openvic-simulation/types/fixed_point/FixedPoint.hpp b/src/openvic-simulation/types/fixed_point/FixedPoint.hpp index 75abefb..8d3a74b 100644 --- a/src/openvic-simulation/types/fixed_point/FixedPoint.hpp +++ b/src/openvic-simulation/types/fixed_point/FixedPoint.hpp @@ -209,6 +209,12 @@ namespace OpenVic { return !is_negative() ? value : -value; } + constexpr fixed_point_t sqrt() const { + return !is_negative() + ? static_cast<int64_t>(NumberUtils::sqrt(static_cast<uint64_t>(value) << PRECISION)) + : 0; + } + // Doesn't account for sign, so -n.abc -> 1 - 0.abc constexpr fixed_point_t get_frac() const { return value & (ONE - 1); diff --git a/src/openvic-simulation/utility/NumberUtils.hpp b/src/openvic-simulation/utility/NumberUtils.hpp index 6211772..d814019 100644 --- a/src/openvic-simulation/utility/NumberUtils.hpp +++ b/src/openvic-simulation/utility/NumberUtils.hpp @@ -9,7 +9,7 @@ namespace OpenVic::NumberUtils { return (num > 0.0) ? (num + 0.5) : (num - 0.5); } - constexpr uint64_t pow(uint64_t base, size_t exponent) { + constexpr uint64_t pow(uint64_t base, std::size_t exponent) { uint64_t ret = 1; while (exponent-- > 0) { ret *= base; @@ -17,11 +17,36 @@ namespace OpenVic::NumberUtils { return ret; } - constexpr int64_t pow(int64_t base, size_t exponent) { + constexpr int64_t pow(int64_t base, std::size_t exponent) { int64_t ret = 1; while (exponent-- > 0) { ret *= base; } return ret; } + + constexpr uint64_t sqrt(uint64_t n) { + uint64_t x = n; + uint64_t c = 0; + uint64_t d = 1ull << 62; + + while (d > n) + d >>= 2; + + for (; d != 0; d >>= 2) { + if (x >= c + d) { + x -= c + d; + c = (c >> 1) + d; + } else { + c >>= 1; + } + } + + //round up + if (x > 0) { + c += 1; + } + + return c; + } } |