File syn_qmath.c¶
File List > src > syntropic > util > syn_qmath.c
Go to the documentation of this file
#if __has_include("syn_config.h")
#include "syn_config.h"
#endif
#if !defined(SYN_USE_QMATH) || SYN_USE_QMATH
#include "../common/syn_defs.h"
#include "syn_qmath.h"
/* ════════════════════════════════════════════════════════════════════════ */
/* Trigonometric — sin, cos, tan */
/* ════════════════════════════════════════════════════════════════════════ */
q16_t q16_sin(q16_t x)
{
/* Normalize x to [-PI, PI] */
while (x > Q16_PI)
x -= Q16_2_PI;
while (x < -Q16_PI)
x += Q16_2_PI;
/* Map to [-PI/2, PI/2] */
if (x > Q16_PI_2) {
x = Q16_PI - x;
} else if (x < -Q16_PI_2) {
x = -Q16_PI - x;
}
/* 7th-order Taylor series: sin(x) ≈ x - x^3/6 + x^5/120 - x^7/5040 */
int64_t x_sq = ((int64_t)x * x) >> 16;
int64_t x_cube = (x_sq * x) >> 16;
int64_t x_five = (x_cube * x_sq) >> 16;
int64_t x_seven = (x_five * x_sq) >> 16;
int64_t term3 = x_cube / 6;
int64_t term5 = x_five / 120;
int64_t term7 = x_seven / 5040;
return (q16_t)(x - term3 + term5 - term7);
}
q16_t q16_cos(q16_t x)
{
return q16_sin(x + Q16_PI_2);
}
q16_t q16_sin_fast(q16_t x)
{
/* Parabolic approximation: sin(x) ≈ B*x + C*x*|x| for x in [-PI, PI] */
while (x > Q16_PI)
x -= Q16_2_PI;
while (x < -Q16_PI)
x += Q16_2_PI;
/* B = 4/PI = 1.2732395447 (83443 in Q16)
C = -4/PI^2 = -0.4052847346 (-26560 in Q16) */
int64_t B = 83443;
int64_t C = -26560;
int64_t abs_x = (x >= 0) ? x : -x;
int64_t x_abs_x = ((int64_t)x * abs_x) >> 16;
int64_t y = ((B * x) >> 16) + ((C * x_abs_x) >> 16);
/* Extra precision smoothing step: y = P*(y*|y| - y) + y where P = 0.225 (14745 in Q16) */
int64_t abs_y = (y >= 0) ? y : -y;
int64_t y_abs_y = (y * abs_y) >> 16;
int64_t P = 14745;
int64_t fine_y = (P * (y_abs_y - y)) >> 16;
return (q16_t)(y + fine_y);
}
q16_t q16_cos_fast(q16_t x)
{
return q16_sin_fast(x + Q16_PI_2);
}
void q16_sincos_fast(q16_t x, q16_t *sin_out, q16_t *cos_out)
{
if (sin_out)
*sin_out = q16_sin_fast(x);
if (cos_out)
*cos_out = q16_cos_fast(x);
}
q16_t q16_tan(q16_t x)
{
q16_t c = q16_cos(x);
/* Guard against division by zero near ±π/2 */
if (c == 0) {
return (q16_sin(x) >= 0) ? INT32_MAX : INT32_MIN;
}
return q16_div(q16_sin(x), c);
}
/* ════════════════════════════════════════════════════════════════════════ */
/* Inverse trigonometric — atan2, asin, acos */
/* ════════════════════════════════════════════════════════════════════════ */
static q16_t atan_core(q16_t x)
{
/* x is in [0, Q16_ONE] */
static const int64_t c1 = 65527; /* 0.999866 */
static const int64_t c3 = -21646; /* -0.330299 */
static const int64_t c5 = 11805; /* 0.180141 */
static const int64_t c7 = -4200; /* -0.064090 */
int64_t x2 = ((int64_t)x * x) >> 16;
int64_t x3 = (x2 * x) >> 16;
int64_t x5 = (x3 * x2) >> 16;
int64_t x7 = (x5 * x2) >> 16;
int64_t result = (c1 * x) >> 16;
result += (c3 * x3) >> 16;
result += (c5 * x5) >> 16;
result += (c7 * x7) >> 16;
return (q16_t)result;
}
q16_t q16_atan2(q16_t y, q16_t x)
{
if (x == 0 && y == 0) {
return 0;
}
q16_t abs_x = q16_abs(x);
q16_t abs_y = q16_abs(y);
q16_t angle;
if (abs_x >= abs_y) {
/* |x| >= |y|: atan(y/x) with |y/x| in [0,1] */
q16_t ratio = q16_div(abs_y, abs_x);
angle = atan_core(ratio);
} else {
/* |y| > |x|: atan(y/x) = π/2 - atan(x/y) */
q16_t ratio = q16_div(abs_x, abs_y);
angle = Q16_PI_2 - atan_core(ratio);
}
/* Quadrant mapping */
if (x < 0) {
angle = Q16_PI - angle;
}
if (y < 0) {
angle = -angle;
}
return angle;
}
q16_t q16_asin(q16_t x)
{
/* Clamp to [-1, 1] */
if (x >= Q16_ONE)
return Q16_PI_2;
if (x <= -Q16_ONE)
return -Q16_PI_2;
/*
* 5th-order Chebyshev approximation for asin(x), |x| <= 1:
* asin(x) ≈ c1*x + c3*x³ + c5*x⁵
*
* Coefficients (Q16.16):
* c1 ≈ 1.0 = 65536
* c3 ≈ 1/6 = 10923
* c5 ≈ 3/40 = 4915
*/
static const q16_t c1 = 65536; /* 1.0 */
static const q16_t c3 = 10923; /* 0.16667 */
static const q16_t c5 = 4915; /* 0.07500 */
int64_t x2 = ((int64_t)x * x) / 65536LL;
int64_t x3 = (x2 * x) / 65536LL;
int64_t x5 = ((x3 * x) / 65536LL) * x / 65536LL;
int64_t result = ((int64_t)c1 * x) / 65536LL;
result += ((int64_t)c3 * x3) / 65536LL;
result += ((int64_t)c5 * x5) / 65536LL;
return (q16_t)result;
}
q16_t q16_acos(q16_t x)
{
return Q16_PI_2 - q16_asin(x);
}
/* ════════════════════════════════════════════════════════════════════════ */
/* Algebraic — sqrt, hypot */
/* ════════════════════════════════════════════════════════════════════════ */
uint32_t syn_isqrt32(uint32_t n)
{
uint32_t root = 0;
uint32_t bit = 1UL << 30;
while (bit > n)
bit >>= 2;
while (bit != 0) {
if (n >= root + bit) {
n -= root + bit;
root = (root >> 1) + bit;
} else {
root >>= 1;
}
bit >>= 2;
}
return root;
}
uint64_t syn_isqrt64(uint64_t n)
{
uint64_t root = 0;
uint64_t bit = (uint64_t)1 << 62;
while (bit > n)
bit >>= 2;
while (bit != 0) {
uint64_t trial = root + bit;
if (n >= trial) {
n -= trial;
root = (root >> 1) + bit;
} else {
root >>= 1;
}
bit >>= 2;
}
return root;
}
q16_t q16_sqrt(q16_t x)
{
if (x <= 0)
return 0;
return (q16_t)syn_isqrt64((uint64_t)(uint32_t)x << 16);
}
q16_t q16_inv(q16_t x)
{
if (x == 0)
return INT32_MAX;
if (x < 0) {
if (x == INT32_MIN)
return 0;
return -q16_inv(-x);
}
int64_t inv = ((int64_t)1 << 32) / x;
if (inv > INT32_MAX)
return INT32_MAX;
return (q16_t)inv;
}
q16_t q16_rsqrt(q16_t x)
{
if (x <= 0)
return 0;
q16_t sqrt_x = q16_sqrt(x);
if (sqrt_x == 0)
return INT32_MAX;
return q16_inv(sqrt_x);
}
q16_t q16_hypot(q16_t x, q16_t y)
{
/*
* Overflow-safe magnitude: sqrt(x² + y²).
*
* If either |x| or |y| exceeds ~181 (Q16), then x*x could overflow
* int64_t accumulation. Pre-shift both values right by 1 in that case,
* then shift the result left by 1.
*/
q16_t ax = q16_abs(x);
q16_t ay = q16_abs(y);
/* 181.0 in Q16 = 11862016. If either exceeds this, we risk overflow
* in the x*x + y*y sum (each product is ~62 bits). */
int shift = 0;
if (ax > 11862016 || ay > 11862016) {
ax >>= 1;
ay >>= 1;
shift = 1;
}
int64_t sum = ((int64_t)ax * ax) + ((int64_t)ay * ay);
/* sum is in Q32.32 (two Q16 values multiplied). We need sqrt in Q16.
* q16_sqrt expects Q16.16 input, but sum is Q32.32.
* Convert: sum >> 16 gives Q16.16, then sqrt gives Q16.16 result. */
if (sum >> 16 > INT32_MAX) {
/* Very large — clamp */
return INT32_MAX;
}
q16_t result = q16_sqrt((q16_t)(sum >> 16));
if (shift) {
result <<= 1;
}
return result;
}
/* ════════════════════════════════════════════════════════════════════════ */
/* Exponential / Logarithmic — exp, log, pow */
/* ════════════════════════════════════════════════════════════════════════ */
q16_t q16_exp(q16_t x)
{
/*
* Range reduction: x = k * ln(2) + r, where k = floor(x / ln2),
* and r is in [0, ln2). Then e^x = 2^k * e^r.
*
* 2^k is a left shift by k (in Q16: Q16_ONE << k).
* e^r is computed via 4th-order minimax polynomial over [0, ln2).
*/
if (x == 0)
return Q16_ONE;
/* Handle negative exponents via e^(-x) = 1/e^x */
if (x < 0) {
q16_t pos = q16_exp(-x);
if (pos == 0)
return INT32_MAX; /* -x overflowed */
return q16_inv(pos);
}
/* Overflow guard: e^10 ≈ 22026, which is near the Q16 integer limit */
if (x > Q16_FROM_INT(10)) {
return INT32_MAX;
}
/* k = x / ln(2), integer part */
int32_t k = (int32_t)(((int64_t)x << 16) / Q16_LN2) >> 16;
/* r = x - k * ln(2), the fractional remainder in [0, ln2) */
q16_t r = x - q16_mul((q16_t)(k * Q16_ONE), Q16_LN2);
/* Clamp r to [0, ln2) — numerical safety */
if (r < 0) {
r = 0;
k--;
}
if (r >= Q16_LN2) {
r -= Q16_LN2;
k++;
}
/*
* e^r ≈ 1 + r + r²/2 + r³/6 + r⁴/24 (4th-order Taylor, r in [0, 0.693])
* Max error < 0.0002 over this range.
*/
int64_t r2 = ((int64_t)r * r) >> 16;
int64_t r3 = (r2 * r) >> 16;
int64_t r4 = (r3 * r) >> 16;
q16_t exp_r = (q16_t)(Q16_ONE + r + r2 / 2 + r3 / 6 + r4 / 24);
/* Multiply by 2^k */
if (k >= 0) {
if (k >= 15)
return INT32_MAX; /* Would overflow Q16 integer range */
return exp_r << k;
} else {
if (k < -16)
return 0;
return exp_r >> (-k);
}
}
q16_t q16_exp_fast(q16_t x)
{
if (x == 0)
return Q16_ONE;
if (x < 0) {
q16_t pos = q16_exp_fast(-x);
if (pos == 0)
return INT32_MAX;
return q16_inv(pos);
}
if (x > Q16_FROM_INT(10))
return INT32_MAX;
int32_t k = (int32_t)(((int64_t)x << 16) / Q16_LN2) >> 16;
q16_t r = x - q16_mul((q16_t)(k * Q16_ONE), Q16_LN2);
if (r < 0) {
r = 0;
k--;
}
if (r >= Q16_LN2) {
r -= Q16_LN2;
k++;
}
q16_t r2 = q16_mul(r, r);
q16_t exp_r = Q16_ONE + r + (r2 >> 1);
if (k >= 0) {
if (k >= 15)
return INT32_MAX;
return exp_r << k;
} else {
if (k < -16)
return 0;
return exp_r >> (-k);
}
}
q16_t q16_log(q16_t x)
{
/*
* Range reduction: normalize x to [1.0, 2.0) by finding the
* highest set bit. Then:
* ln(x) = ln(m * 2^k) = k * ln(2) + ln(m)
* where m is in [1, 2).
*
* ln(m) is computed via polynomial approximation on [1, 2).
*/
if (x <= 0)
return INT32_MIN; /* Undefined */
if (x == Q16_ONE)
return 0;
/* Find k such that x = m * 2^k, where m is in [Q16_ONE, 2*Q16_ONE) */
uint32_t ux = (uint32_t)x;
int32_t k = 0;
/* Normalize to [Q16_ONE, 2*Q16_ONE) */
while (ux >= (uint32_t)(2 * Q16_ONE)) {
ux >>= 1;
k++;
}
while (ux < (uint32_t)Q16_ONE) {
ux <<= 1;
k--;
}
/* m is now in [Q16_ONE, 2*Q16_ONE), stored in ux as Q16.16 */
q16_t m = (q16_t)ux;
/*
* ln(m) for m in [1, 2) via transformation y = (m - 1) / (m + 1):
* ln(m) = 2 * (y + y³/3 + y⁵/5 + y⁷/7)
* Rapidly convergent for y in [0, 1/3].
*/
q16_t y = q16_div(m - Q16_ONE, m + Q16_ONE);
int64_t y2 = ((int64_t)y * y) >> 16;
int64_t y3 = (y2 * y) >> 16;
int64_t y5 = (y3 * y2) >> 16;
int64_t y7 = (y5 * y2) >> 16;
int64_t ln_m = ((int64_t)y + y3 / 3 + y5 / 5 + y7 / 7) * 2;
/* ln(x) = k * ln(2) + ln(m) */
return q16_mul(Q16_FROM_INT(k), Q16_LN2) + (q16_t)ln_m;
}
q16_t q16_log_fast(q16_t x)
{
if (x <= 0)
return INT32_MIN;
if (x == Q16_ONE)
return 0;
uint32_t ux = (uint32_t)x;
int32_t k = 0;
while (ux >= (uint32_t)(2 * Q16_ONE)) {
ux >>= 1;
k++;
}
while (ux < (uint32_t)Q16_ONE) {
ux <<= 1;
k--;
}
q16_t m = (q16_t)ux;
q16_t y = m - Q16_ONE;
q16_t y2 = q16_mul(y, y);
q16_t ln_m = y - (y2 >> 1);
return q16_mul(Q16_FROM_INT(k), Q16_LN2) + ln_m;
}
q16_t q16_pow(q16_t base, q16_t exp)
{
if (base <= 0)
return 0;
if (exp == 0)
return Q16_ONE;
if (exp == Q16_ONE)
return base;
return q16_exp(q16_mul(exp, q16_log(base)));
}
q16_t q16_floor(q16_t x)
{
return (q16_t)((uint32_t)x & 0xFFFF0000u);
}
q16_t q16_ceil(q16_t x)
{
return (q16_t)(((uint32_t)x + 0x0000FFFFu) & 0xFFFF0000u);
}
q16_t q16_round(q16_t x)
{
return q16_floor(x + Q16_HALF);
}
q16_t q16_poly_eval(const q16_t *coeffs, uint8_t n, q16_t x)
{
if (coeffs == NULL || n == 0)
return 0;
q16_t res = coeffs[n - 1];
for (int i = (int)n - 2; i >= 0; i--) {
res = q16_mul(res, x) + coeffs[i];
}
return res;
}
/* ════════════════════════════════════════════════════════════════════════ */
/* String I/O */
/* ════════════════════════════════════════════════════════════════════════ */
size_t q16_to_str(q16_t val, char *buf, size_t buf_len, uint8_t decimals)
{
if (buf == NULL || buf_len == 0)
return 0;
if (decimals > 4)
decimals = 4;
char tmp[16];
size_t pos = 0;
/* Handle negative */
int32_t sign = 0;
uint32_t uval;
if (val < 0) {
sign = 1;
/* Use unsigned to handle INT32_MIN correctly */
uval = (uint32_t)(-(int64_t)val);
} else {
uval = (uint32_t)val;
}
/* Integer part */
uint32_t int_part = uval >> 16;
uint32_t frac_bits = uval & 0xFFFF;
/* Build integer digits (reverse order) */
char int_buf[8];
size_t int_len = 0;
if (int_part == 0) {
int_buf[int_len++] = '0';
} else {
while (int_part > 0) {
int_buf[int_len++] = (char)('0' + (int_part % 10));
int_part /= 10;
}
}
/* Assemble: sign + reversed integer digits */
if (sign) {
if (pos < sizeof(tmp) - 1)
tmp[pos++] = '-';
}
while (int_len > 0) {
if (pos < sizeof(tmp) - 1)
tmp[pos++] = int_buf[--int_len];
}
/* Fractional part */
if (decimals > 0) {
if (pos < sizeof(tmp) - 1)
tmp[pos++] = '.';
/* Multiply fractional bits by 10 repeatedly to extract digits */
uint8_t d;
for (d = 0; d < decimals; d++) {
frac_bits *= 10;
uint8_t digit = (uint8_t)(frac_bits >> 16);
frac_bits &= 0xFFFF;
if (pos < sizeof(tmp) - 1)
tmp[pos++] = (char)('0' + digit);
}
}
tmp[pos] = '\0';
/* Copy to output buffer */
if (pos >= buf_len) {
buf[0] = '\0';
return 0;
}
size_t i;
for (i = 0; i <= pos; i++) {
buf[i] = tmp[i];
}
return pos;
}
size_t q16_from_str(const char *str, q16_t *out)
{
if (str == NULL || out == NULL)
return 0;
const char *p = str;
int32_t sign = 1;
/* Optional sign */
if (*p == '-') {
sign = -1;
p++;
} else if (*p == '+') {
p++;
}
/* Must have at least one digit */
if (*p < '0' || *p > '9')
return 0;
/* Integer part */
int32_t int_part = 0;
while (*p >= '0' && *p <= '9') {
int_part = int_part * 10 + (*p - '0');
p++;
}
/* Fractional part */
uint32_t frac_q16 = 0;
if (*p == '.') {
p++;
/*
* Parse fractional digits. Each digit d at position i contributes
* d * 65536 / 10^i to the Q16 fractional part. We accumulate by
* multiplying the running total by 10 and adding the digit, then
* dividing at the end.
*/
uint32_t frac_num = 0;
uint32_t frac_den = 1;
while (*p >= '0' && *p <= '9') {
frac_num = frac_num * 10 + (uint32_t)(*p - '0');
frac_den *= 10;
p++;
if (frac_den >= 100000)
break; /* Limit precision to avoid overflow */
}
if (frac_den > 0) {
frac_q16 = (uint32_t)(((uint64_t)frac_num << 16) / frac_den);
}
}
*out = sign * (q16_t)((int32_t)(int_part << 16) | (int32_t)frac_q16);
return (size_t)(p - str);
}
#endif /* SYN_USE_QMATH */