feat: integer and fixed point square root functions

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;
+	}
 }