Math: Complex: Calculate complex phase angle with sofm_atan2()

src/include/sof/audio/format.h

@@ -70,6 +70,9 @@
 /* Convert fractional Qnx.ny number x to float */
 #define Q_CONVERT_QTOF(x, ny) ((float)(x) / ((int64_t)1 << (ny)))
 
+/* Convert fractional Qnx.ny number x to double */
+#define Q_CONVERT_QTOD(x, ny) ((double)(x) / ((int64_t)1 << (ny)))
+
 /* A more clever macro for Q-shifts */
 #define Q_SHIFT(x, src_q, dst_q) ((x) >> ((src_q) - (dst_q)))
 #define Q_SHIFT_RND(x, src_q, dst_q) \

src/include/sof/math/trig.h

@@ -383,4 +383,17 @@ static inline int16_t acos_fixed_16b(int32_t cdc_acos_th)
 	return sat_int16(Q_SHIFT_RND(th_acos_fxp, 29, 13));
 }
 
+/**
+ * sofm_atan2_32b() - Four-quadrant arctangent using degree-9 Remez minimax polynomial
+ * @param y Imaginary component (sine) in Q1.31 format.
+ * @param x Real component (cosine) in Q1.31 format.
+ * @return Angle in Q3.29 radians, range [-pi, +pi].
+ *
+ * Uses the Horner-form polynomial:
+ *   atan(z) = z * (C0 + z^2 * (C1 + z^2 * (C2 + z^2 * C3)))
+ * with Remez minimax coefficients on [0, 1].
+ * Maximum error ~0.001 degrees (1.94e-5 radians).
+ */
+int32_t sofm_atan2_32b(int32_t y, int32_t x);
+
 #endif /* __SOF_MATH_TRIG_H__ */

src/math/CMakeLists.txt

@@ -11,6 +11,10 @@ if(CONFIG_CORDIC_FIXED)
   list(APPEND base_files trig.c)
 endif()
 
+if(CONFIG_MATH_ATAN2)
+  list(APPEND base_files atan2.c)
+endif()
+
 if(CONFIG_MATH_LUT_SINE_FIXED)
   list(APPEND base_files lut_trig.c)
 endif()

src/math/Kconfig

@@ -9,6 +9,16 @@ config CORDIC_FIXED
 	  Select this to enable sin(), cos(), asin(), acos(),
 	  and cexp() functions as 16 bit and 32 bit versions.
 
+config MATH_ATAN2
+	bool "Four-quadrant arctangent function"
+	default n
+	help
+	  Select this to enable sofm_atan2_32b(y, x) function. It computes
+	  the four-quadrant arctangent using a polynomial approximation.
+	  Input arguments y and x are Q1.31 format and the output angle
+	  is Q3.29 format with range [-pi, pi]. The maximum approximation
+	  error is ~0.001 degrees.
+
 config MATH_LUT_SINE_FIXED
 	bool "Lookup table based sine function"
 	default n
@@ -98,6 +108,9 @@ config MATH_DECIBELS
 config MATH_COMPLEX
 	bool "Operations for complex numbers"
 	default n
+	select CORDIC_FIXED
+	select MATH_ATAN2
+	select SQRT_FIXED
 	help
 	  Select this to enable functions for complex numbers
 	  arithmetic such as conversions to/from polar format.

src/math/atan2.c

@@ -0,0 +1,114 @@
+// SPDX-License-Identifier: BSD-3-Clause
+//
+// Copyright(c) 2026 Intel Corporation.
+//
+// Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
+
+/**
+ * \file math/atan2.c
+ * \brief Four-quadrant arctangent function using polynomial approximation
+ */
+
+#include <rtos/symbol.h>
+#include <sof/audio/format.h>
+#include <sof/math/trig.h>
+#include <stdint.h>
+
+/*
+ * Degree-9 Remez minimax polynomial for atan(z) in first octant, z in [0, 1]:
+ *
+ *   atan(z) = z * (C0 + z^2 * (C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4))))
+ *
+ * Coefficients are in Q3.29 format, computed via Remez exchange algorithm
+ * to minimize the maximum approximation error over [0, 1].
+ * Maximum approximation error is ~0.0011 degrees (1.94e-5 radians).
+ */
+#define SOFM_ATAN2_C0 536890772	 /* Q3.29, +1.000036992319 */
+#define SOFM_ATAN2_C1 -178298711 /* Q3.29, -0.332107229582 */
+#define SOFM_ATAN2_C2 99794340	 /* Q3.29, +0.185881443393 */
+#define SOFM_ATAN2_C3 -49499604	 /* Q3.29, -0.092200197922 */
+#define SOFM_ATAN2_C4 12781063	 /* Q3.29, +0.023806584771 */
+
+/**
+ * sofm_atan2_32b() - Compute four-quadrant arctangent of y/x
+ * @param y Imaginary component in Q1.31 format
+ * @param x Real component in Q1.31 format
+ * @return Angle in Q3.29 radians, range [-pi, +pi]
+ *
+ * Uses degree-9 Remez minimax polynomial for the core atan(z) computation
+ * where z = min(|y|,|x|) / max(|y|,|x|) is reduced to [0, 1] range.
+ * Octant and quadrant corrections extend the result to full [-pi, +pi].
+ */
+int32_t sofm_atan2_32b(int32_t y, int32_t x)
+{
+	int32_t abs_x;
+	int32_t abs_y;
+	int32_t num;
+	int32_t den;
+	int32_t z;
+	int32_t z2;
+	int32_t acc;
+	int32_t angle;
+	int swap;
+
+	/* Return zero for origin */
+	if (x == 0 && y == 0)
+		return 0;
+
+	/* Take absolute values, handle INT32_MIN overflow */
+	abs_x = (x > 0) ? x : ((x == INT32_MIN) ? INT32_MAX : -x);
+	abs_y = (y > 0) ? y : ((y == INT32_MIN) ? INT32_MAX : -y);
+
+	/* Reduce to first octant: z = min/max ensures z in [0, 1] */
+	if (abs_y <= abs_x) {
+		num = abs_y;
+		den = abs_x;
+		swap = 0;
+	} else {
+		num = abs_x;
+		den = abs_y;
+		swap = 1;
+	}
+
+	/* z = num / den in Q1.31, den is always > 0 here */
+	z = sat_int32(((int64_t)num << 31) / den);
+
+	/*
+	 * Horner evaluation of degree-9 Remez minimax polynomial:
+	 *   atan(z) = z * (C0 + z^2 * (C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4))))
+	 *
+	 * z is Q1.31, coefficients are Q3.29.
+	 * Multiplications: Q1.31 * Q3.29 = Q4.60, >> 31 -> Q3.29.
+	 */
+
+	/* z^2: Q1.31 * Q1.31 = Q2.62, >> 31 -> Q1.31 */
+	z2 = (int32_t)(((int64_t)z * z) >> 31);
+
+	/* Innermost: C3 + z^2 * C4 */
+	acc = (int32_t)(((int64_t)z2 * SOFM_ATAN2_C4) >> 31) + SOFM_ATAN2_C3;
+
+	/* C2 + z^2 * (C3 + z^2 * C4) */
+	acc = (int32_t)(((int64_t)z2 * acc) >> 31) + SOFM_ATAN2_C2;
+
+	/* C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4)) */
+	acc = (int32_t)(((int64_t)z2 * acc) >> 31) + SOFM_ATAN2_C1;
+
+	/* C0 + z^2 * (C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4))) */
+	acc = (int32_t)(((int64_t)z2 * acc) >> 31) + SOFM_ATAN2_C0;
+
+	/* angle = z * acc: Q1.31 * Q3.29 = Q4.60, >> 31 -> Q3.29 */
+	angle = (int32_t)(((int64_t)z * acc) >> 31);
+
+	/* If |y| > |x|, use identity: atan(y/x) = pi/2 - atan(x/y) */
+	if (swap)
+		angle = PI_DIV2_Q3_29 - angle;
+
+	/* Map to correct quadrant based on signs of x and y */
+	if (x < 0)
+		angle = (y >= 0) ? PI_Q3_29 - angle : -(PI_Q3_29 - angle);
+	else if (y < 0)
+		angle = -angle;
+
+	return angle;
+}
+EXPORT_SYMBOL(sofm_atan2_32b);

src/math/complex.c

@@ -9,6 +9,7 @@
 #include <sof/audio/format.h>
 #include <sof/math/icomplex32.h>
 #include <sof/math/sqrt.h>
+#include <sof/math/trig.h>
 #include <stdint.h>
 
 /* sofm_icomplex32_to_polar() - Convert (re, im) complex number to polar. */
@@ -17,8 +18,6 @@ void sofm_icomplex32_to_polar(struct icomplex32 *complex, struct ipolar32 *polar
 	struct icomplex32 c = *complex;
 	int64_t squares_sum;
 	int32_t sqrt_arg;
-	int32_t acos_arg;
-	int32_t acos_val;
 
 	/* Calculate square of magnitudes Q1.31, result is Q2.62 */
 	squares_sum = (int64_t)c.real * c.real + (int64_t)c.imag * c.imag;
@@ -27,16 +26,8 @@ void sofm_icomplex32_to_polar(struct icomplex32 *complex, struct ipolar32 *polar
 	sqrt_arg = Q_SHIFT_RND(squares_sum, 62, 30);
 	polar->magnitude = sofm_sqrt_int32(sqrt_arg); /* Q2.30 */
 
-	/* Avoid divide by zero and ambiguous angle for a zero vector. */
-	if (polar->magnitude == 0) {
-		polar->angle = 0;
-		return;
-	}
-
-	/* Calculate phase angle with acos( complex->real / polar->magnitude) */
-	acos_arg = sat_int32((((int64_t)c.real) << 29) / polar->magnitude); /* Q2.30 */
-	acos_val = acos_fixed_32b(acos_arg);				    /* Q3.29 */
-	polar->angle = (c.imag < 0) ? -acos_val : acos_val;
+	/* Angle */
+	polar->angle = sofm_atan2_32b(c.imag, c.real); /* Q3.29 */
 }
 EXPORT_SYMBOL(sofm_icomplex32_to_polar);
 

test/ztest/unit/math/basic/complex/CMakeLists.txt

@@ -22,6 +22,7 @@ target_sources(app PRIVATE
 	${SOF_ROOT}/src/math/complex.c
 	${SOF_ROOT}/src/math/sqrt_int32.c
 	${SOF_ROOT}/src/math/trig.c
+	${SOF_ROOT}/src/math/atan2.c
 )
 
 # Link math library for standard math functions

test/ztest/unit/math/basic/complex/test_complex_polar.c

@@ -15,7 +15,7 @@ LOG_MODULE_REGISTER(test_complex_polar, LOG_LEVEL_INF);
 
 #define COMPLEX_ABS_TOL 1.2e-8
 #define MAGNITUDE_ABS_TOL 7.1e-8
-#define ANGLE_ABS_TOL 4.4e-5
+#define ANGLE_ABS_TOL 2.0e-5
 
 /**
  * @brief Test complex to polar conversion function
@@ -35,7 +35,7 @@ ZTEST(math_complex, test_icomplex32_to_polar)
 	double magnitude, angle;
 	double delta_mag, delta_ang;
 	double magnitude_scale_q30 = 1.0 / 1073741824.0; /* 1.0 / 2^30 */
-	double angle_scale_q29 = 1.0 / 536870912.0; /* 1.0 / 2^29 */
+	double angle_scale_q29 = 1.0 / 536870912.0;	 /* 1.0 / 2^29 */
 	double delta_mag_max = 0;
 	double delta_ang_max = 0;
 	int i;
@@ -118,11 +118,11 @@ ZTEST(math_complex, test_ipolar32_to_complex)
 
 	/* Re-run worst cases to print info */
 	sofm_ipolar32_to_complex(&polar_real_max, &complex);
-	printf("delta_real_max = %g at (%d, %d) -> (%d, %d)\n", delta_real_max,
+	printf(" INFO - delta_real_max = %g at (%d, %d) -> (%d, %d)\n", delta_real_max,
 	       polar_real_max.magnitude, polar_real_max.angle, complex.real, complex.imag);
 
 	sofm_ipolar32_to_complex(&polar_imag_max, &complex);
-	printf("delta_imag_max = %g at (%d, %d) -> (%d, %d)\n", delta_imag_max,
+	printf(" INFO - delta_imag_max = %g at (%d, %d) -> (%d, %d)\n", delta_imag_max,
 	       polar_imag_max.magnitude, polar_imag_max.angle, complex.real, complex.imag);
 }
 

test/ztest/unit/math/basic/trigonometry/CMakeLists.txt

@@ -21,6 +21,7 @@ target_sources(app PRIVATE
 	trig_test.c
 	${SOF_ROOT}/src/math/trig.c
 	${SOF_ROOT}/src/math/lut_trig.c
+	${SOF_ROOT}/src/math/atan2.c
 )
 
 # Link math library for standard math functions