aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/openvic-simulation/types/fixed_point/FixedPoint.hpp6
-rw-r--r--src/openvic-simulation/utility/NumberUtils.hpp29
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;
+ }
}