matlab bilinear函数 C实现（双线性变换）

bilinear

void bilinear(int n, double *a, double *b, double *c, double *d, double fs)
{
    double t = 1 / fs;
    double r = sqrt(t);
    double *eye_a = (double *)malloc(n * n * sizeof(double));
    double *t1 = (double *)malloc(n * n * sizeof(double));
    double *t2 = (double *)malloc(n * n * sizeof(double));
    double *t2_rinv = (double *)malloc(n * n * sizeof(double));
    double *ad = (double *)malloc(n * n * sizeof(double));
    double *bd = (double *)malloc(n * n * sizeof(double));
    double *cd = (double *)malloc(n * n * sizeof(double));
    double *dd = (double *)malloc(n * n * sizeof(double));
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            if (i == j)
                eye_a[i * n + j] = 1;
            else
                eye_a[i * n + j] = 0;
            t1[i * n + j] = eye_a[i * n + j] + a[i * n + j] * t / 2;
            t2[i * n + j] = eye_a[i * n + j] - a[i * n + j] * t / 2;
            t2_rinv[i * n + j] = eye_a[i * n + j] - a[i * n + j] * t / 2;
        }
    }
    
    rinv(t2_rinv,n);
    // t2_rinv = inver(t2, n);
    trmul(t2_rinv, t1, n, n, n, ad);
    trmul(t2_rinv, b, n, n, 1, bd); // t2\b = t2^-1*b
    trmul(c, t2_rinv, 1, n, n, cd);
    trmul(cd, b, 1, n, 1, dd);
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            bd[i * n + j] = bd[i * n + j] * t / r;
            cd[i * n + j] = cd[i * n + j] * r;
            dd[i * n + j] = dd[i * n + j] * t / 2 + *d;
            a[i * n + j] = ad[i * n + j];
        }
    }
    for (int i = 0; i < n; i++)
    {
        b[i] = bd[i];
        c[i] = cd[i];
    }
    *d = dd[0];

    free(eye_a);
    free(t1);
    free(t2);
    free(t2_rinv);
    free(ad);
    free(bd);
    free(cd);
    free(dd);
}

其中rinv函数是求矩阵的逆，代码如下：

/*************************************************************************
 * @brief   :  矩阵求逆
 * @inparam :  a		矩阵A
 *			  n		矩阵A的阶数
 * @outparam:  a		逆矩阵
 *************************************************************************/
void rinv(double a[], int n)
{
    int *is, *js, i, j, k, l, u, v;
    double d, p;
    is = (int *)malloc(n * sizeof(int));
    js = (int *)malloc(n * sizeof(int));

    for (k = 0; k <= n - 1; k++)
    {
        d = 0.0;
        for (i = k; i <= n - 1; i++)
            for (j = k; j <= n - 1; j++)
            {
                l = i * n + j;
                p = fabs(a[l]);
                if (p > d)
                {
                    d = p;
                    is[k] = i;
                    js[k] = j;
                }
            }
        if (fabs(d) < MINEPS)
        {
            free(is);
            free(js);
        }

        if (is[k] != k)
            for (j = 0; j <= n - 1; j++)
            {
                u = k * n + j;
                v = is[k] * n + j;

                p = a[u];
                a[u] = a[v];
                a[v] = p;
            }
        if (js[k] != k)
            for (i = 0; i <= n - 1; i++)
            {
                u = i * n + k;
                v = i * n + js[k];
                p = a[u];
                a[u] = a[v];
                a[v] = p;
            }
        l = k * n + k;
        a[l] = 1.0 / a[l];
        for (j = 0; j <= n - 1; j++)
            if (j != k)
            {
                u = k * n + j;
                a[u] = a[u] * a[l];
            }
        for (i = 0; i <= n - 1; i++)
            if (i != k)
                for (j = 0; j <= n - 1; j++)
                    if (j != k)
                    {
                        u = i * n + j;
                        a[u] = a[u] - a[i * n + k] * a[k * n + j];
                    }
        for (i = 0; i <= n - 1; i++)
            if (i != k)
            {
                u = i * n + k;
                a[u] = -a[u] * a[l];
            }
    }
    for (k = n - 1; k >= 0; k--)
    {
        if (js[k] != k)
            for (j = 0; j <= n - 1; j++)
            {
                u = k * n + j;
                v = js[k] * n + j;
                p = a[u];
                a[u] = a[v];
                a[v] = p;
            }
        if (is[k] != k)
            for (i = 0; i <= n - 1; i++)
            {
                u = i * n + k;
                v = i * n + is[k];
                p = a[u];
                a[u] = a[v];
                a[v] = p;
            }
    }
    free(is);
    free(js);
}

还有trmul为矩阵相乘，代码如下：

*************************************************************************
 * @brief   :  实数矩阵相乘
 * @inparam :  a		矩阵A
 *			  b		矩阵B
 *			  m		矩阵A与乘积矩阵C的行数
 *			  n		矩阵A的行数,矩阵B的列数
 *			  k		矩阵B与乘积矩阵C的列数
 * @outparam:  c		乘积矩阵 C=AB
 *************************************************************************/
void trmul(double a[], double b[], int m, int n, int k, double c[])
{
    int i, j, l, u;
    for (i = 0; i <= m - 1; i++)
        for (j = 0; j <= k - 1; j++)
        {
            u = i * k + j;
            c[u] = 0.0;
            for (l = 0; l <= n - 1; l++)
                c[u] = c[u] + a[i * n + l] * b[l * k + j];
        }
}

测试：已和matlab函数对比验证，放心食用