Test hypotenuse

README.md

@@ -21,7 +21,8 @@
 * Output angle expressed as binary angle measurement `[0x0, 0xffff]`
 * 10 iterations of CORDIC vectoring mode to find angle
 * Angle accurate to within 5 bits (`32 / 0x10000` approx. `0.0031 rad`)
-* Hypotenuse not used, but can be calculated by dividing result `x` by gain $\prod_{i=0}^{9} \sqrt{1 + 2^{-2 \cdot i}} \approx 1.64676$
+* Hypotenuse optional
+	- Calculated by dividing result `x` by gain $\prod_{i=0}^{9} \sqrt{1 + 2^{-2 \cdot i}} \approx 1.64676$
 
 # Branchless ARMv4 implementation
 
@@ -44,4 +45,7 @@ Arctangent
 	- 75 cycles for 10 CORDIC iterations
 	- 2 cycles for negative angle adjustment
 
+* 3 cycles for Q8 hypotenuse gain adjustment (if used)
+	- See [header](arm-test/trig_approx.hpp) for hypotenuse use and gain adjustment
+
 [ARMv4 source including table](arm-test/trig_approx.arm.s)

arm-test/arm-test.cpp

@@ -7,11 +7,7 @@
 
 #include "nocash_printf.hpp"
 
-extern "C" {
-	int32_t tan_approx(uint32_t x);
-	int32_t sec_approx(uint32_t x);
-	uint32_t cordic_atan2(int32_t x, int32_t y);
-} // extern "C"
+#include "trig_approx.hpp"
 
 static void set_bg_color(uint32_t rgb15)
 {
@@ -47,9 +43,37 @@ static auto test_sec_angle(uint32_t x)
 	return abs(calculated - approximated) <= 2;
 }
 
+static auto test_atan2(int32_t x, int32_t y)
+{
+	auto [angle, gained_hyp] = cordic_atan2_hyp(x, y);
+	auto hyp = adj_cordic_hyp(gained_hyp);
+
+	auto actual_angle = int32_t(round(atan2(y, x) / (2 * M_PI) * 0x10000));
+	if (actual_angle < 0) { actual_angle += 0x10000; }
+	auto fpsq = [](int32_t x) {
+		auto xf = float(x) / (1 << 16);
+		return xf * xf;
+	};
+	auto ahf = sqrt(fpsq(x) + fpsq(y)) * (1 << 16);
+	auto actual_hyp = int32_t(round(ahf));
+
+	if (abs(actual_angle - angle) > 32) {
+		set_bg_color(0x001f);
+		nocash_printf("Failed atan2(%d / %d) angle: actual %d appr %d",
+			y, x, actual_angle, angle);
+		nocash_break(true);
+	}
+
+	if (float(abs(actual_hyp - hyp)) / actual_hyp > 0.01) {
+		set_bg_color(0x001f);
+		nocash_printf("Failed atan2(%d / %d) hyp: actual %d appr %d",
+			y, x, actual_hyp, hyp);
+		nocash_break(true);
+	}
+};
+
 int main(int argc, char** argv)
 {
-#if 0
 	// Test all angles in range [0, pi/4)
 	for (int32_t x = 0x0; x < 0x2000; x++) {
 		if (!test_tan_angle(x) || !test_sec_angle(x)) {
@@ -68,23 +92,7 @@ int main(int argc, char** argv)
 		}
 	}
 
-	set_bg_color(0x03e0);
-	nocash_printf("Passed all angles in range (0xe000, 0xffff], [0x0000, 0x2000)");
-#endif
-	nocash_printf("Running CORDIC atan2 test, will take a while...");
-
-	auto test_atan2 = [](int32_t x, int32_t y) {
-		auto actual = int32_t(round(atan2(y, x) / (2 * M_PI) * 0x10000));
-		if (actual < 0) { actual += 0x10000; }
-		auto approx = cordic_atan2(x, y);
-		if (abs(actual - approx) > 32) {
-			set_bg_color(0x001f);
-			nocash_printf("Failed atan2(%d / %d): actual %d appr %d",
-				y, x, actual, approx);
-			nocash_break(true);
-		}
-	};
-
+	// Basic atan2 tests
 	test_atan2(50, -30);
 	test_atan2(50, 30);
 	test_atan2(30, 50);
@@ -94,6 +102,11 @@ int main(int argc, char** argv)
 	test_atan2(-30, -50);
 	test_atan2(30, -50);
 
+	set_bg_color(0x03e0);
+	nocash_printf("Passed all angles in range (0xe000, 0xffff], [0x0000, 0x2000)");
+
+	nocash_printf("Running CORDIC atan2 test, will take a while...");
+
 	for (int32_t x = 0x100; x < 0x1000; x += 0x101) {
 		for (int32_t y = 0x10000; y < 0x100000; y += 0x101) {
 			test_atan2(x, y);

arm-test/trig_approx.arm.s

@@ -146,8 +146,8 @@ trig_lut:
 .section .iwram, "ax", %progbits
 .align 2
 .arm
-.global cordic_atan2
-cordic_atan2:
+.global _cordic_atan2
+_cordic_atan2:
 
 @ Adjust angle to range (-pi/4, pi/4)
 @ Input: r0 x, r1 y

arm-test/trig_approx.hpp

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <cstdint>
+#include <tuple>
+
+// Implementations in trig_approx.arm.s
+extern "C" {
+
+	// Input r0 angle
+	// Output r0 Q18 tan(angle)
+	int32_t tan_approx(uint32_t angle);
+
+	// Input r0 angle
+	// Output r0 Q18 tan(angle)
+	int32_t sec_approx(uint32_t angle);
+
+	// Input r0 x, r1 y
+	// Output r0 angle
+	uint32_t _cordic_atan2(int32_t x, int32_t y);
+} // extern "C"
+
+static inline uint32_t cordic_atan2(int32_t x, int32_t y)
+{
+	return _cordic_atan2(x, y);
+}
+
+static inline int32_t adj_cordic_hyp(int32_t x)
+{
+	constexpr auto gain = (int32_t)round(float(1 << 8) / 1.64676);
+	return (x * gain) >> 8;
+}
+
+static inline auto cordic_atan2_hyp(int32_t x, int32_t y)
+{
+	// Constrain x to r0, y to r1
+	register int32_t r0 __asm__ ("r0") = x;
+	register int32_t r1 __asm__ ("r1") = y;
+
+	// Call assembly _cordic_atan2
+	// Input r0 x, r1 y
+	// Output r0 angle, r1 gained hypotenuse
+	asm volatile("bl _cordic_atan2"
+		: "+r" (r0), "+r" (r1)
+		: "r" (r0), "r" (r1)
+		: "r2", "r3", "lr");
+
+	// Return values
+	auto angle = int32_t{r0};
+	auto hyp = int32_t{r1};
+
+	return std::make_pair(angle, hyp);
+}