imath.h

/*
 * Copyright (C) 2023 ML!PA Consulting GmbH
 *
 * This file is subject to the terms and conditions of the GNU Lesser
 * General Public License v2.1. See the file LICENSE in the top level
 * directory for more details.
 */
 
#ifndef IMATH_H
#define IMATH_H
 
#include <stdint.h>
 
#ifdef __cplusplus
extern "C" {
#endif
 
#define SINI_PERIOD      0x8000 
#define SINI_MAX         0x1000 
#define SINI_MIN        -0x1000 
static inline  __attribute__((always_inline))
int32_t _ihelp(int32_t x)
{
    int32_t y;
    static const int32_t qN = 13,
                         qA = 12,
                          B = 19900,
                          C = 3516;
 
    x = x << (31 - qN);         /* Mask with PI                 */
    x = x >> (31 - qN);         /* Note: SIGNED shift! (to qN)  */
    x = x * x >> (2 * qN - 14); /* x=x^2 To Q14                 */
 
    y = B - (x * C >> 14);      /* B - x^2*C                    */
    y = (1 << qA)               /* A - x^2*(B-x^2*C)            */
      - (x * y >> 16);
 
    return y;
}
 
static inline int32_t fast_sini(int32_t x)
{
    static const int32_t qN = 13;
 
    int32_t c = x << (30 - qN);
    int32_t y = _ihelp(x - (1 << qN));
 
    return c >= 0 ? y :-y;
}
 
static inline int32_t fast_cosi(int32_t x)
{
    static const int32_t qN = 13;
 
    int32_t c = (x + (SINI_PERIOD >> 2)) << (30 - qN);
    int32_t y = _ihelp(x);
 
    return c >= 0 ? y :-y;
}
 
static inline unsigned sqrti(unsigned x)
{
    if (x <= 1) {
        return x;
    }
 
    /* initial estimate */
    unsigned y0 = x >> 1;
    unsigned y1 = (y0 + x / y0) >> 1;
 
    while (y1 < y0) {
        y0 = y1;
        y1 = (y0 + x / y0) >> 1;
    }
 
    return y0;
}
 
static inline uint32_t powi(unsigned x, unsigned y)
{
    uint32_t res = 1;
 
    while (y--) {
        res *= x;
    }
 
    return res;
}
 
#ifdef __cplusplus
}
#endif
 
#endif /* IMATH_H */