超好用的纯C语言矩阵运算库

超好用的纯C语言矩阵运算库 easyMatrix


最近开发基于异构传感器的异步定位数据的卡尔曼滤波,因为最终要用在一个老爷DSP上,已有代码都是C写的,不想研究这种老爷设备下的C++调用,Eigen这超好用的矩阵运算库算是没法用了。随便搜了几个C矩阵库发现不是超难用就是有bug,比如csdn某小哥写的matrix库,矩阵维数一大,逆矩阵计算就出错。项目结束后,想着可能很多嵌入式开发人员也许有类似的需求,决定自己还是写一个操作友好的matrix库,工具虽小,好用就行。
支持矩阵的加、减、乘法运算,转置、逆矩阵、伴随矩阵、LU分解、行列式计算、线性方程求解等功能
废话不说,上代码:
github地址: https://github.com/fellylanma/easyMatrix

//使用例子
int main() {
	//create matrix on stack
 	float val1[] = {1,2,3,4,5,6,7,8,9};
	float val2[] = {1,1,1,1,1,1,1,1,1};
	CREATE_MATRIX_ONSTACK(3,3,A,val1);
	CREATE_MATRIX_ONSTACK(3,3,B,val2);
	CREATE_MATRIX_ONSTACK(3,3,C,NULL);
	addMatrix(&A,&B,&C);
	dumpMatrix(&C);
    printf("det is:%f\n",detMatrix(&A));
	int x = A.rows;
	int y = A.cols;
	printf("element[1,1] is:%f\n",A.element[1*x+1]);
	//create matrix on heap and you need to delete the pointer
	CREATE_DYNAMIC_MATRIX_ONHEAP(x,y,D,val1);
	CREATE_DYNAMIC_MATRIX_ONHEAP(x,y,E,val2);
	CREATE_DYNAMIC_MATRIX_ONHEAP(x,y,F,NULL);
	multiMatrix(D,E,&C);
	multiMatrix(D,E,F);

	DELETE_DYNAMIC_MATRIX(D);
	DELETE_DYNAMIC_MATRIX(E);
	DELETE_DYNAMIC_MATRIX(F);
}

easyMatrix.h

#ifndef _MAGRIDE_PLANNING_EASYMATRIX_H_
#define _MAGRIDE_PLANNING_EASYMATRIX_H_

#include <stdlib.h>
typedef unsigned char uint8;
typedef float DATA_TYPE;


#define CREATE_MATRIX_ONSTACK(x,y,matrix,initval) \
struct easyMatrix matrix;\
DATA_TYPE val##x##N##y##N##matrix[x*y];\
    matrix.rows = x;\
    matrix.cols = y;\
    matrix.element = val##x##N##y##N##matrix;\
    if(initval!=NULL) setMatrix(initval, &(matrix))

#define CREATE_DYNAMIC_MATRIX_ONHEAP(x,y,matrix,initval) \
struct easyMatrix *matrix = (struct easyMatrix*)malloc(sizeof(struct easyMatrix));\
matrix->rows = x;\
matrix->cols = y;\
matrix->element = (DATA_TYPE*) malloc(sizeof(DATA_TYPE)*(x)*(y));\
if(initval!=NULL) setMatrix(initval, (matrix))

#define DELETE_DYNAMIC_MATRIX(matrix) \
    free((matrix)->element);\
    free(matrix)

struct easyMatrix {\
    uint8 rows,cols;\
    DATA_TYPE* element;
};\

struct easyMatrix* setMatrix(DATA_TYPE* const a,struct easyMatrix* c);
struct easyMatrix* copyMatrix(struct easyMatrix* const a,struct easyMatrix* c);
struct easyMatrix* transMatrix(struct easyMatrix* const a,struct easyMatrix* c);

DATA_TYPE detMatrix(struct easyMatrix* const a);

DATA_TYPE invMatrix(struct easyMatrix* const a, struct easyMatrix*b);

struct easyMatrix* scaleMatrix(DATA_TYPE, struct easyMatrix* const a, struct easyMatrix*);

struct easyMatrix* addMatrix(const struct easyMatrix* const a, const struct easyMatrix *const  b, struct easyMatrix * c);

struct easyMatrix* leftMatrix(uint8, uint8, struct easyMatrix* const a, struct easyMatrix* b);

struct easyMatrix* subMatrix(struct easyMatrix* const a, struct easyMatrix* const  b, struct easyMatrix* c);

struct easyMatrix* multiMatrix(struct easyMatrix* const a, struct easyMatrix* const b, struct easyMatrix* c);

struct easyMatrix* zerosMatrix(struct easyMatrix* e);

struct easyMatrix* eyesMatrix(struct easyMatrix* e);

void dumpMatrix(struct easyMatrix* const e);

struct easyMatrix* adjMatrix(struct easyMatrix* const a,struct easyMatrix* c);

struct easyMatrix* getLUMatrix(struct easyMatrix* const A, struct easyMatrix* L,struct easyMatrix* U) ;

struct easyMatrix* invLMatrix(struct easyMatrix* const L, struct easyMatrix* L_inv) ;
struct easyMatrix* invUMatrix(struct easyMatrix* const U, struct easyMatrix* U_inv) ;

struct easyMatrix* solveEquationMatrix(const struct easyMatrix* const A,const struct easyMatrix* const Y, struct easyMatrix* X) ;


DATA_TYPE fastDetMatrix(struct easyMatrix* const in) ;
#endif//_MAGRIDE_PLANNING_EASYMATRIX_H_

#easyMatrix.c

#include <stdlib.h>
#include <float.h>
#include "easyMatrix.h"

int isFiniteNumber(double d) {
    return (d<=DBL_MAX&&d>=-DBL_MAX);
}
struct easyMatrix* setMatrix(DATA_TYPE * const a,struct easyMatrix* c) {
    uint8 x = c->rows;
    uint8 y = c->cols;
    int t = x*y;
    for(int i=0;i<t;++i) {
        c->element[i] = a[i];
    }
    return c;
}

struct easyMatrix* copyMatrix(struct easyMatrix* const a,struct easyMatrix* c) {
    if(a->rows != c->rows) return NULL;
    if(a->cols != c->cols) return NULL;
    int t = a->rows*a->cols;
    for(int i=0;i<t;++i) {
        c->element[i] = a->element[i];
    }
    return c;
}

struct easyMatrix* transMatrix(struct easyMatrix* const a,struct easyMatrix* c) {
    if(a->rows != c->cols) return NULL;
    if(a->cols != c->rows) return NULL;
    int index = 0;
    int index_src = 0;
    for(uint8 ii=0;ii<a->cols;++ii) {
        index_src=ii;
        for(uint8 jj=0;jj<a->rows;++jj) {
            //c->element[index] = a->element[jj*a->cols+ii];
            c->element[index] = a->element[index_src];
            index++;
            index_src+=a->cols;
        }
    }
    return c;
}

struct easyMatrix* leftMatrix(uint8 x_i,uint8 y_i, struct easyMatrix* const in, struct easyMatrix* out) {
    if(in->rows != in->cols) return NULL;
    if(out->rows != out->cols) return NULL;
    if(in->rows != (out->rows+1)) return NULL;
    int index = 0;
    int index_src = 0;
    uint8 x =in->rows;
    uint8 y =in->cols;
    for(uint8 kk=0;kk<x;++kk) {
        for(uint8 ww=0;ww<y;++ww) {
            if(!(kk==x_i||ww==y_i)) {
                //out->element[index] = in->element[kk*y+ww];
                out->element[index] = in->element[index_src];
                index++;
            }
            index_src++;
        }
    }
    return out;
}
struct easyMatrix* adjMatrix(struct easyMatrix* const in, struct easyMatrix* out) {
    if(in->rows != out->cols) return NULL;
    if(in->cols != out->rows) return NULL;
    int index = 0;
    uint8 x = in->rows;
    uint8 y = in->cols;
    CREATE_DYNAMIC_MATRIX_ONHEAP(x-1,y-1,ret,NULL);
    signed char sign1 = 1;
    signed char sign2 = 1;
    for(uint8 ii=0;ii<x;++ii) {
        sign2 = sign1;
        index = ii;
        for(uint8 jj=0;jj<y;++jj) {
            leftMatrix(ii,jj,in,ret);
            //out->element[jj*y+ii] = sign2*detMatrix(ret);
            out->element[index] = sign2*detMatrix(ret);
            sign2 = - sign2;    
            index+=y;
        }
        
        sign1 = - sign1;
    }
    DELETE_DYNAMIC_MATRIX(ret);
    return out;
}

DATA_TYPE invMatrix(struct easyMatrix *const in , struct easyMatrix * out) {
    if(in->cols!=in->rows) return 0;
    if(in->rows != out->cols) return 0;
    if(in->cols != out->rows) return 0;
    uint8 N = in->cols;
    CREATE_DYNAMIC_MATRIX_ONHEAP(N,N,L,NULL);
    CREATE_DYNAMIC_MATRIX_ONHEAP(N,N,LINV,NULL);
    CREATE_DYNAMIC_MATRIX_ONHEAP(N,N,U,NULL);
    CREATE_DYNAMIC_MATRIX_ONHEAP(N,N,UINV,NULL);
    getLUMatrix(in,L,U);
    invLMatrix(L,LINV);
    invUMatrix(U,UINV);
    multiMatrix(UINV,LINV,out);
    double s = 1;
    for(int i = 0;i<N;i++) 
        s *= U->element[i*N+i];   
    /*
    adjMatrix(in,out);
    DATA_TYPE scale = detMatrix(in);
    if(scale<1e-5&&scale>-1e-5) return 0.0;
    scale = 1/scale;
    scaleMatrix(scale,out,out);
*/
    DELETE_DYNAMIC_MATRIX(L);
    DELETE_DYNAMIC_MATRIX(U);
    DELETE_DYNAMIC_MATRIX(LINV);
    DELETE_DYNAMIC_MATRIX(UINV);
    return isFiniteNumber(s);
}

struct easyMatrix* getLUMatrix(struct easyMatrix* const A, struct easyMatrix* L,struct easyMatrix* U) {
    int row=0;
    DATA_TYPE s = 0;
    uint8 N = A->cols;
    int t = N*N;
    for(int i =0;i<t;i++) {
        L->element[i] = 1e-20;
        U->element[i] = 1e-20;
    }
    for(int i=0;i<N;i++) {
        L->element[i*N+i] = 1.0;
    }
    for(int i=0;i<N;i++) {
        for(int j=i;j<N;j++) {
            s = 0.0;
            for(int k=0;k<i;++k) {
                s+=L->element[i*N+k]*U->element[k*N+j];
            }
            U->element[i*N+j]= A->element[i*N+j] - s; 
        }
        for (int j = i + 1;j < N;j++) {
            s = 0.0;
            for (int k = 0; k < i; k++)
            {
                s += L->element[j*N+k] * U->element[k*N+i];
            }
            L->element[j*N+i] = (A->element[j*N+i] - s) / U->element[i*N+i];      //按列计算l值
        }
    }
    return L;

}

struct easyMatrix* invLMatrix(struct easyMatrix* const L, struct easyMatrix* L_inv) { 
    uint8 N = L->cols;
    DATA_TYPE s;
    int t = N*N;
    for(int i =0;i<t;i++) {
        L_inv->element[i] = 1e-13;
    }
    for (uint8 i = 0;i < N;i++)  {
        L_inv->element[i*N+i] = 1;
    }
    for (uint8 i= 1;i < N;i++) {
        for (uint8 j = 0;j < i;j++) {
            s = 0;
            for (uint8 k = 0;k < i;k++) {
                s += L->element[i*N+k] * L_inv->element[k*N+j];
            }
            L_inv->element[i*N+j] = -s;
        }
    }
    return L_inv;
}
struct easyMatrix* invUMatrix(struct easyMatrix* const U, struct easyMatrix* U_inv) { 
    uint8 N = U->cols;
    DATA_TYPE s;
    int t = N*N;
    for(int i =0;i<t;i++) {
        U_inv->element[i] = 1e-13;
    }
 for (uint8 i = 0;i < N;i++)                    //按列序,列内按照从下到上,计算u的逆矩阵
    {
        U_inv->element[i*N+i] = 1 / U->element[i*N+i];
    }
    for (uint8 i = 1;i < N;i++) {
        for (int j = i - 1;j >=0;j--) {
            s = 0;
            for (uint8 k = j + 1;k <= i;k++) {
                s += U->element[j*N+k] * U_inv->element[k*N+i];
            }
            U_inv->element[j*N+i] = -s / U->element[j*N+j];
        }
    }
    return U_inv;
}
DATA_TYPE fastDetMatrix(struct easyMatrix* const in) {
    uint8 x = in->rows;
    uint8 y = in->cols;
    if(x!=y) return 0;
    if(x==0 ) return 0;
    if(x==1 ) return in->element[0];
    DATA_TYPE *a =in->element;
    if(x==2) return(a[0]*a[3]-a[1]*a[2]);
    int N = x;
    CREATE_DYNAMIC_MATRIX_ONHEAP(N,N,L,NULL);
    CREATE_DYNAMIC_MATRIX_ONHEAP(N,N,U,NULL);
    getLUMatrix(in,L,U);
    double s = 1;
    for(int i = 0;i<N;i++) 
        s *= U->element[i*N+i];
    DELETE_DYNAMIC_MATRIX(L);
    DELETE_DYNAMIC_MATRIX(U);
    return s;
}

DATA_TYPE detMatrix(struct easyMatrix* const in) {
    uint8 x = in->rows;
    uint8 y = in->cols;
    if(x!=y) return 0;
    if(x==0 ) return 0;
    if(x==1 ) return in->element[0];
    DATA_TYPE *a =in->element;
    if(x==2) return(a[0]*a[3]-a[1]*a[2]);

    DATA_TYPE result = 0;
    signed char sign = 1;
    CREATE_DYNAMIC_MATRIX_ONHEAP(x-1,y-1,ret,NULL);
    for(uint8 i=0;i<x;++i) {
        leftMatrix(0,i,in,ret);
        result += sign*a[i]*detMatrix(ret);
        sign = - sign;
    }
    DELETE_DYNAMIC_MATRIX(ret);
    return result;
}

struct easyMatrix* addMatrix(const struct easyMatrix* const a,const struct easyMatrix* const b, struct easyMatrix* c) {
    if(a->cols != b->cols) return NULL;
    if(a->rows != b->rows) return NULL;
    struct easyMatrix* obj = (struct easyMatrix*)a;
    int t = obj->rows*obj->cols;
    for(int i=0;i<t;++i) {
        c->element[i] = obj->element[i]+b->element[i];
    }
    return c;
}

struct easyMatrix* subMatrix(struct easyMatrix* const a, struct easyMatrix* const b, struct easyMatrix* c) {
    if(a->cols != b->cols) return NULL;
    if(a->rows != b->rows) return NULL;
    struct easyMatrix* obj = (struct easyMatrix*)a;
    int t = obj->rows*obj->cols;
    for(int i=0;i<t;++i) {
        c->element[i] = obj->element[i]-b->element[i];
    }
    return c;
}

struct easyMatrix* scaleMatrix(DATA_TYPE scale, struct easyMatrix* const a, struct easyMatrix* b) {
    int t = a->cols*a->rows;
    for (int i = 0;i<t;++i) {
        b->element[i] = a->element[i]*scale;
    }
    return b;
}

struct easyMatrix* multiMatrix(struct easyMatrix* const a,struct easyMatrix* const b, struct easyMatrix* c) {
    if(NULL==c) return NULL;
    if(c == a || c == b) return NULL;
    if(a->cols != b->rows) return NULL;
    int count = 0;
    int t_cnt = 0;
    int z_cnt = 0;
    uint8 x = a->rows;
    uint8 y = a->cols;
    uint8 z = b->cols;
    for(uint8 i = 0;i<x;++i) {
        for(uint8 k = 0;k<z;++k) {
            c->element[count] = 0;
            z_cnt = 0;
            for(uint8 j = 0;j<y;++j) {
                c->element[count] += a->element[t_cnt+j]*b->element[z_cnt+k];
                z_cnt += z;
            }
            count++;
        }
        t_cnt+=y;
    }
    return c;
}

struct easyMatrix* zerosMatrix(struct easyMatrix* e) {
    int t = e->cols*e->rows;
    for(int i=0;i<t;++i) {
        e->element[i] = 0;
    }
    return e;
}

struct easyMatrix* eyesMatrix(struct easyMatrix* e) {
    if(e->rows != e->cols) return NULL;
    zerosMatrix(e);
    int index = 0;
    for(uint8 i=0;i<e->rows;++i) {
        e->element[index] = 1.0;
        index+=(e->cols);
        ++index;
    }
    return e;
}

struct easyMatrix* solveEquationMatrix(const struct easyMatrix* const A,const struct easyMatrix* const Y, struct easyMatrix* X) { 
    CREATE_DYNAMIC_MATRIX_ONHEAP(A->rows,A->cols,AINV,NULL);
    invMatrix(A,AINV);
    multiMatrix(AINV,Y,X);
    DELETE_DYNAMIC_MATRIX(AINV);
    return X;
}
void dumpMatrix(struct easyMatrix* const e) {
    int count = 0;
    int x = e->rows;
    int y = e->cols;
    printf("cols is:%d, rows is:%d\n",x,y);
    for(uint8 i = 0;i<x;++i) {
        for(uint8 j = 0;j<y;++j) {
            printf("%8f,",e->element[count]);
            ++count;
        }
        printf("\n");
    }
    return;
}
评论 9
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值