add sqrt

include/fixmath/fixed.hpp

@@ -62,6 +62,7 @@ class fixed final {
 	static constexpr fixed from_raw(uraw_t v) { return fixed(from_raw_t{}, v); }
 
 	constexpr raw_t raw() const { return value; }
+	constexpr uraw_t uraw() const { return static_cast<uraw_t>(value); }
 
 	constexpr fixed() = default;
 	constexpr fixed(const fixed&) = default;
@@ -90,8 +91,7 @@ class fixed final {
 	{}
 };
 
-using Fix32 = fixed<fixed_policy<int64_t, 32, arithmetic_mode::SaturationMode, rounding_mode::RoundToEven>>;
-
 } // namespace fixmath
 
 #include "fixed_impl.inl"
+#include "fixed_math.inl"

include/fixmath/fixed_math.inl

@@ -0,0 +1,76 @@
+/* This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */
+
+// intentionally omit header guard
+// DO NOT MANULLY INCLUDE THIS FILE
+
+namespace fixmath {
+
+template<FixedPolicy policy>
+constexpr fixed<policy> sqrt(fixed<policy> a) {
+	using fixed = fixed<policy>;
+	using raw_t = typename fixed::raw_t;
+	using uraw_t = typename fixed::uraw_t;
+	if constexpr (policy::strict_mode) {
+		if (FIXMATH_UNLIKELY(a.is_nan())) {
+			return fixed::nan();
+		}
+		if (FIXMATH_UNLIKELY(a.is_inf() && a > 0)) {
+			return fixed::max_inf();
+		}
+		if (FIXMATH_UNLIKELY(a < 0)) {
+			FIXMATH_ERROR("sqrt(<0)");
+			return fixed::nan();
+		}
+	}
+	if constexpr (policy::saturation_mode) {
+		if (FIXMATH_UNLIKELY(a < fixed(0))) {
+			FIXMATH_ERROR("sqrt(<0)");
+			return fixed::min_sat();
+		}
+	}
+	if (a == 0) {
+		return 0;
+	}
+	uraw_t value = a.uraw();
+	uraw_t root = 0;
+	uraw_t remainder = 0;
+	raw_t start_i = _fm_clz(value) >> 1;
+	value <<= (start_i << 1);
+	for (raw_t i = start_i; i < fixed::ALL_BITS / 2; ++i) {
+		root <<= 1;
+		remainder = (remainder << 2) | (value >> (fixed::ALL_BITS - 2));
+		uraw_t tester = (root << 1) | 1;
+		if (tester <= remainder) {
+			root |= 1;
+			remainder -= tester;
+		}
+		value <<= 2;
+	}
+	for (raw_t i = 0; i < fixed::FRACTION_BITS / 2; ++i) {
+		root <<= 1;
+		remainder <<= 2;
+		uraw_t tester = (root << 1) | 1;
+		if (tester <= remainder) {
+			root |= 1;
+			remainder -= tester;
+		}
+	}
+	uraw_t result = root;
+	if constexpr (policy::rounding) {
+		root <<= 1;
+		remainder <<= 2;
+		uraw_t tester = (root << 1) | 1;
+		if (tester <= remainder) {
+			if (remainder == tester) {
+				result += result & 1;
+			} else {
+				result += 1;
+			}
+		}
+	}
+	return fixed::from_raw(result);
+}
+
+} // namespace fixmath

include/fixmath/fixmath_clz.inl

@@ -8,14 +8,14 @@
 namespace fixmath {
 
 #if FIXMATH_LINUX
-inline int _fm_clzll(uint64_t x) {
+inline int _fm_clz(uint64_t x) {
 	if (x == 0) {
 		return 64;
 	}
 	return __builtin_clzll(x);
 }
 #elif FIXMATH_WIN && FIXMATH_64BIT
-inline int _fm_clzll(uint64_t value) {
+inline int _fm_clz(uint64_t value) {
 	unsigned long leading_zero = 0;
 	if (_BitScanReverse64(&leading_zero, value)) {
 		return static_cast<int>(63 - leading_zero);
@@ -24,7 +24,7 @@ inline int _fm_clzll(uint64_t value) {
 	}
 }
 #elif FIXMATH_WIN && FIXMATH_32BIT
-inline int _fm_clzll(uint64_t value) {
+inline int _fm_clz(uint64_t value) {
 	unsigned long leading_zero = 0;
 	if (_BitScanReverse(&leading_zero, (unsigned long)(value >> 32))) {
 		return static_cast<int>(31 - leading_zero);
@@ -35,7 +35,7 @@ inline int _fm_clzll(uint64_t value) {
 	}
 }
 #else
-inline int _fm_clzll(uint64_t x) {
+inline int _fm_clz(uint64_t x) {
 	int result = 0;
 	if (x == 0) return 64;
 	while (!(x & 0xF000000000000000ULL)) { result += 4; x <<= 4; }

include/fixmath/fixmath_softdiv128.inl

@@ -19,7 +19,7 @@ inline uint64_t _softudiv128(uint64_t u1, uint64_t u0, uint64_t v, uint64_t* r)
 	uint64_t rhat;                                    // A remainder
 	int32_t s;                                        // Shift amount for normalization
 
-	s = _fm_clzll(v);
+	s = _fm_clz(v);
 	if (s > 0) {
 		// Normalize the divisor.
 		v = v << s;

tests/unit_tests.cpp

@@ -16,6 +16,7 @@ const double pi = std::acos(-1);
 
 namespace {
 
+using Fix32 = fixed<fixed_policy<int64_t, 32, arithmetic_mode::SaturationMode, rounding_mode::RoundToEven>>;
 
 using i32 = fixmath::int32_t;
 using i64 = fixmath::int64_t;
@@ -25,7 +26,7 @@ using i64l = std::numeric_limits<i64>;
 using f32l = std::numeric_limits<float>;
 using f64l = std::numeric_limits<double>;
 
-using fix32l = std::numeric_limits<fixmath::Fix32>;
+using fix32l = std::numeric_limits<Fix32>;
 
 const double ABSERROR = static_cast<double>(fix32l::epsilon());
 
@@ -305,6 +306,18 @@ TEST(FIXMATH, COMPARE) {
 	EXPECT_EQ(Fix32(-ABSERROR) != Fix32(ABSERROR), true);
 }
 
+TEST(FIX32, SQUARE) {
+	EXPECT_FIX_NEAR(sqrt(Fix32(55554288)), sqrt(55554288));
+	EXPECT_FIX_NEAR(sqrt(Fix32(1.1)), sqrt(1.1));
+	EXPECT_FIX_DOMAIN_ERROR(sqrt(Fix32(-1.1)));
+	EXPECT_FIX_DOMAIN_ERROR(sqrt(Fix32(-ABSERROR)));
+	EXPECT_FIX_DOMAIN_ERROR(sqrt(Fix32::min_fix()));
+	EXPECT_FIX_NEAR(sqrt(Fix32(0)), 0);
+	EXPECT_FIX_NEAR(sqrt(Fix32(1.1)), sqrt(1.1));
+	EXPECT_FIX_NEAR(sqrt(Fix32(4)), sqrt(4));
+	EXPECT_FIX_NEAR(sqrt(Fix32(123456789.987654321)), sqrt(123456789.987654321));
+}
+
 template<class T, class U> requires FixedImplicitBinaryOperable<T, U>
 constexpr int func(T, U) { return 1; }