这里实现的矩阵库是将矩阵都分配在栈内存中的,这使得我在进行较大量的矩阵运算时将栈给撑爆了,所以更好的办法是使用malloc动态分配内存。
查看改进的矩阵库:
https://github.com/colourfate/math_matrix
矩阵库介绍:
https://blog.csdn.net/Egean/article/details/78387277
以下是原文。
由于要在stm32上实现矩阵运算,所以结合网上代码实现了一个C语言矩阵库,进行一些矩阵的基本运算,包括:转置,加减,乘法,求逆,拼接等,测试环境是MDK5。先给出下载地址:点击这里。
首先是头文件math_matrix.h:
#ifndef _MATRIX_H_
#define _MATRIX_H_
#include "sys.h"
struct matrix_t{
float *m;
u8 row;
u8 column;
};
#define MATRIX_INIT(a,b,c,d) \
a.m=b; \
a.row=c; \
a.column=d
int8_t matrix_t_T(struct matrix_t *A, const struct matrix_t *B);
void matrix_t_show(struct matrix_t *M);
int8_t matrix_t_plus(struct matrix_t *A, const struct matrix_t *B,
const struct matrix_t *C, int8_t mode);
int8_t matrix_t_mul(struct matrix_t *A, const struct matrix_t *B,
const struct matrix_t *C, int8_t mode);
int8_t matrix_t_inv(struct matrix_t *A, const struct matrix_t *B);
int8_t matrix_t_copy(struct matrix_t *A, const struct matrix_t *B);
int8_t matrix_t_eye(struct matrix_t *A);
int8_t matrix_t_k(struct matrix_t *A, float k, const struct matrix_t *B);
int8_t matrix_t_concat(struct matrix_t *A, const struct matrix_t *B,
const struct matrix_t *C, u8 mode);
int8_t matrix_t_transport(struct matrix_t *A, const struct matrix_t *B,
u8 x1, u8 x2, u8 y1, u8 y2);
#endif
struct matrix_t为矩阵结构体,其中matrix_t.m指向一个二维数组,也就是我们的矩阵。由于C语言中二维数组作为函数传参必须指定列数,就像这样:
void fun(float a[][10])
{
...
}
这种传参限制了矩阵的使用,所以这里的二维数组都使用一维数组进行索引,具体索引方法见math_matrix.c文件的实现。
需要注意的是matrix_t.m是一个指针,它指向一个二维数组,而不是matrix_t中包含了一个二维数组,这么设计的原因是因为矩阵大小是不定的,若放到matrix_t中则结构体的大小也是不定的,而C语言不能根据数组的大小动态改变结构体的大小,一个结构体在定义它的时候,它的大小已经固定了,因此只能使用这种指针的形式。这就意味着在初始化时必须预先定义好一个二维数组,然后将matrix_t.m指向它。这里的MATRIX_INIT宏的作用就是如此,将定义好的二维数组传入,就能够方便的初始化matrix_t结构体。
下面是math_matrix.c的具体实现:
/* 基本的矩阵运算,使用结构体,一部分来源于网络:
* http://blog.csdn.net/linaijunix/article/details/50358617
* 做了一些更改,将所有的二重指针换为了一重指针,数据类型做了一些替换,
* 并重新定义了一些函数以支持结构体的运算,函数传参中不需要传入行列数了,
* 而且运算之前进行了行列数的检查,当行列数不符合运算规则时直接返回负数
* 2017/10/23 by colourfate
*/
#include "math_matrix.h"
#include "sys.h"
#include <math.h>
#include <stdio.h>
static void matrix_T(float *a_matrix, const float *b_matrix, u16 krow, u16 kline)
// a_matrix:转置后的矩阵
// b_matrix:转置前的矩阵
// krow :行数
// kline :列数
{
int k, k2;
for (k = 0; k < krow; k++)
{
for(k2 = 0; k2 < kline; k2++)
{
//a_matrix[k2][k] = b_matrix[k][k2];
a_matrix[k2*krow+k] = b_matrix[k*kline+k2];
}
}
}
static void matrix_plus(float *a_matrix, const float *b_matrix, const float *c_matrix,
u8 krow, u8 kline, int8_t ktrl)
// a_matrix=b_matrix+c_matrix
// krow :行数
// kline :列数
// ktrl :大于0: 加法 不大于0:减法
{
int k, k2;
for (k = 0; k < krow; k++)
{
for(k2 = 0; k2 < kline; k2++)
{
//a_matrix[k][k2] = b_matrix[k][k2]
// + ((ktrl > 0) ? c_matrix[k][k2] : -c_matrix[k][k2]);
a_matrix[k*kline+k2] = b_matrix[k*kline+k2]
+ ((ktrl > 0) ? c_matrix[k*kline+k2] : -c_matrix[k*kline+k2]);
}
}
}
static void matrix_mul(float *a_matrix, const float *b_matrix, const float *c_matrix,
u8 krow, u8 kline, u8 kmiddle, int8_t ktrl)
// a_matrix=b_matrix*c_matrix
// krow :b的行数
// kline :c的列数
// kmiddle: b的列数和c的行数
// ktrl : 大于0:两个正数矩阵相乘 不大于0:正数矩阵乘以负数矩阵
{
int k, k2, k4;
float stmp;
for (k = 0; k < krow; k++)
{
for (k2 = 0; k2 < kline; k2++)
{
stmp = 0.0;
for (k4 = 0; k4 < kmiddle; k4++)
{
//stmp += b_matrix[k][k4] * c_matrix[k4][k2];
stmp += b_matrix[k*kmiddle+k4] * c_matrix[k4*kline+k2];
}
//a_matrix[k][k2] = stmp;
a_matrix[k*kline+k2] = stmp;
}
}
if (ktrl <= 0)
{
for (k = 0; k < krow; k++)
{
for (k2 = 0; k2 < kline; k2++)
{
//a_matrix[k][k2] = -a_matrix[k][k2];
a_matrix[k*kline+k2] = -a_matrix[k*kline+k2];
}
}
}
}
static u8 matrix_inv(float *a_matrix, u8 ndimen)
// a_matrix:矩阵
// ndimen :维数
{
float tmp, tmp2, b_tmp[20], c_tmp[20];
int k, k1, k2, k3, j, i, j2, i2, kme[20], kmf[20];
i2 = j2 = 0;
for (k = 0; k < ndimen; k++)
{
tmp2 = 0.0;
for (i = k; i < ndimen; i++)
{
for (j = k; j < ndimen; j++)
{
//if (fabs(a_matrix[i][j] ) <= fabs(tmp2))
if (fabs(a_matrix[i*ndimen+j] ) <= fabs(tmp2))
continue;
//tmp2 = a_matrix[i][j];
tmp2 = a_matrix[i*ndimen+j];
i2 = i;
j2 = j;
}
}
if (i2 != k)
{
for (j = 0; j < ndimen; j++)
{
//tmp = a_matrix[i2][j];
//a_matrix[i2][j] = a_matrix[k][j];
//a_matrix[k][j] = tmp;
tmp = a_matrix[i2*ndimen+j];
a_matrix[i2*ndimen+j] = a_matrix[k*ndimen+j];
a_matrix[k*ndimen+j] = tmp;
}
}
if (j2 != k)
{
for (i = 0; i < ndimen; i++)
{
//tmp = a_matrix[i][j2];
//a_matrix[i][j2] = a_matrix[i][k];
//a_matrix[i][k] = tmp;
tmp = a_matrix[i*ndimen+j2];
a_matrix[i*ndimen+j2] = a_matrix[i*ndimen+k];
a_matrix[i*ndimen+k] = tmp;
}
}
kme[k] = i2;
kmf[k] = j2;
for (j = 0; j < ndimen; j++)
{
if (j == k)
{
b_tmp[j] = 1.0 / tmp2;
c_tmp[j] = 1.0;
}
else
{
//b_tmp[j] = -a_matrix[k][j] / tmp2;
//c_tmp[j] = a_matrix[j][k];
b_tmp[j] = -a_matrix[k*ndimen+j] / tmp2;
c_tmp[j] = a_matrix[j*ndimen+k];
}
//a_matrix[k][j] = 0.0;
//a_matrix[j][k] = 0.0;
a_matrix[k*ndimen+j] = 0.0;
a_matrix[j*ndimen+k] = 0.0;
}
for (i = 0; i < ndimen; i++)
{
for (j = 0; j < ndimen; j++)
{
//a_matrix[i][j] = a_matrix[i][j] + c_tmp[i] * b_tmp[j];
a_matrix[i*ndimen+j] = a_matrix[i*ndimen+j] + c_tmp[i] * b_tmp[j];
}
}
}
for (k3 = 0; k3 < ndimen; k3++)
{
k = ndimen - k3 - 1;
k1 = kme[k];
k2 = kmf[k];
if (k1 != k)
{
for (i = 0; i < ndimen; i++)
{
//tmp = a_matrix[i][k1];
//a_matrix[i][k1] = a_matrix[i][k];
//a_matrix[i][k] = tmp;
tmp = a_matrix[i*ndimen+k1];
a_matrix[i*ndimen+k1] = a_matrix[i*ndimen+k];
a_matrix[i*ndimen+k] = tmp;
}
}
if (k2 != k)
{
for(j = 0; j < ndimen; j++)
{
//tmp = a_matrix[k2][j];
//a_matrix[k2][j] = a_matrix[k][j];
//a_matrix[k][j] = tmp;
tmp = a_matrix[k2*ndimen+j];
a_matrix[k2*ndimen+j] = a_matrix[k*ndimen+j];
a_matrix[k*ndimen+j] = tmp;
}
}
}
return (0);
}
/* 矩阵拷贝函数,A = B,两矩阵行列必须相同
* @A: 目标矩阵
* @B: 源矩阵
* @row: A和B的行数
* @colum: A和B的列数
*/
static void matrix_copy(float *A, const float *B, u8 row, u8 column)
{
int i,j;
for(i=0; i<row; i++){
for(j=0; j<column; j++){
A[column*i+j] = B[column*i+j];
}
}
}
/* 生成单位矩阵
* @A: 生成的单位矩阵
* @dimen: 矩阵维度
*/
static void matrix_eye(float *A, u8 dimen)
{
int i,j;
for(i=0; i<dimen; i++){
for(j=0; j<dimen; j++){
if(i==j){
A[dimen*i+j] = 1;
}else{
A[dimen*i+j] = 0;
}
}
}
}
/* 常数乘以一个矩阵,A = k * B
* @A: 目标矩阵
* @B: 源矩阵
* @k: 系数k
* @row: B的行数
* @column: B的列数
*/
static void matrix_k(float *A, float k, const float *B, u8 row, u8 column)
{
int i,j;
for(i=0; i<row; i++){
for(j=0; j<column; j++){
A[column*i+j] = k * B[column*i+j];
}
}
}
/* 矩阵拼接函数,两矩阵必须列数相等
* vertical: A = |B|,horizontal: A = |B C|
* |C|
* @A: 目标矩阵
* @B: 源矩阵1
* @C: 源矩阵2
* @a_row, a_column, b_row, b_column: B,C矩阵的行数和列数
* @mode: 为1表示竖向拼接,为0表示横向拼接
@ return: 非零表示拼接失败,0表示成功
*/
static int8_t matrix_concat(float *A, const float *B, const float *C,
u8 b_row, u8 b_column, u8 c_row, u8 c_column, int8_t mode)
{
int i, j, k;
if(mode == 0){
if(b_row != c_row){
return -1;
}
for(i=0; i<b_row; i++){
for(j=0, k=0; j<b_column; j++, k++){
A[(b_column+c_column)*i+k] = B[b_column*i+j];
}
for(j=0; j<c_column; j++, k++){
A[(b_column+c_column)*i+k] = C[c_column*i+j];
}
}
}else if(mode == 1){
if(b_column != c_column){
return -1;
}
matrix_copy(A, B, b_row, b_column);
matrix_copy(A+b_row*b_column, C, c_row, c_column);
}else{
return -2;
}
}
/* 显示一个矩阵
* @M: 要显示的矩阵
* @row: M的行数
* @colum: M的列数
*/
static void matrix_show(float *M, u8 row, u8 column)
{
int i,j;
for(i=0; i<row; i++){
printf("|");
for(j=0; j<column; j++){
printf("%f ", *(M+column*i+j));
}
printf("|\r\n");
}
}
/* A = B的转置,A的行数必须等于B的列数,A的列数必须等于B的行数
* @A:转置后的矩阵
* @B:转置前的矩阵
* return: 返回0表示成功,返回非零表示失败
*/
int8_t matrix_t_T(struct matrix_t *A, const struct matrix_t *B)
{
if(A->column != B->row || A->row != B->column){
return -2;
}
matrix_T(A->m, B->m, B->row, B->column);
return 0;
}
void matrix_t_show(struct matrix_t *M)
{
matrix_show(M->m, M->row, M->column);
}
/* A = B + C,B和C的行列数必须相等
* @mode: 大于0为加法,小于零为减法
*/
int8_t matrix_t_plus(struct matrix_t *A, const struct matrix_t *B,
const struct matrix_t *C, int8_t mode)
{
if(B->row != C->row || B->column != C->column){
return -1;
}
if(A->row != B->row || A->column != B->column){
return -2;
}
matrix_plus(A->m, B->m, C->m, B->row, B->column, mode);
return 0;
}
/* A = BC, B的列数必须等于C的行数
* @mode: 大于0:两个正数矩阵相乘 不大于0:正数矩阵乘以负数矩阵
*/
int8_t matrix_t_mul(struct matrix_t *A, const struct matrix_t *B,
const struct matrix_t *C, int8_t mode)
{
if(B->column != C->row){
return -1;
}
if(A->row != B->row || A->column != C->column){
return -2;
}
matrix_mul(A->m, B->m, C->m, B->row, C->column, B->column, mode);
return 0;
}
/* A = B的逆, B必须是方阵
*/
int8_t matrix_t_inv(struct matrix_t *A, const struct matrix_t *B)
{
if(B->row != B->column){
return -1;
}
if(A->row != B->row || A->column != B->column){
return -2;
}
matrix_copy(A->m, B->m, B->row, B->column);
matrix_inv(A->m, A->row);
return 0;
}
/* A = B
*/
int8_t matrix_t_copy(struct matrix_t *A, const struct matrix_t *B)
{
if(A->row != B->row || A->column != B->column){
return -2;
}
matrix_copy(A->m, B->m, B->row, B->column);
return 0;
}
int8_t matrix_t_eye(struct matrix_t *A)
{
if(A->row != A->column){
return -2;
}
matrix_eye(A->m, A->row);
return 0;
}
/* A = kB
*/
int8_t matrix_t_k(struct matrix_t *A, float k, const struct matrix_t *B)
{
if(A->row != B->row || A->column != B->column){
return -2;
}
matrix_k(A->m, k, B->m, B->row, B->column);
return 0;
}
int8_t matrix_t_concat(struct matrix_t *A, const struct matrix_t *B,
const struct matrix_t *C, u8 mode)
{
return matrix_concat(A->m, B->m, C->m, B->row, B->column, C->row, C->column, mode);
}
/* A = B(x1:x2, y1:y2)
*/
int8_t matrix_t_transport(struct matrix_t *A, const struct matrix_t *B,
u8 x1, u8 x2, u8 y1, u8 y2)
{
int i,j;
if(x1>x2 || y1>y2){
return -1;
}
if(A->row != x2-x1+1 || A->column != y2-y1+1){
return -2;
}
for(i=0; i<A->row; i++){
for(j=0; j<A->column; j++){
A->m[i*A->column+j] = B->m[(x1+i)*B->column+y1+j];
}
}
return 0;
}
使用方法如下:
#include "math_matrix.h"
int main(void)
{
float A[2][2];
float B[2][2] = {
{1.0, 2.0},
{3.0, 4.0}
};
struct matrix_t AA, BB;
MATRIX_INIT(AA, *A, 2, 2);
MATRIX_INIT(BB, *B, 2, 2);
// AA = BB
matrix_t_T(&AA, &BB);
matrix_t_show(&AA);
}
需要注意的是这里的
MATRIX_INIT(AA, *A, 2, 2);
需要将二维数组A解引用一次,因为A的数据类型是数组指针,指向一个数组,解引用一
次后就得到了该数组的首地址。当然这只是为了让编译器不报错的措施,不管是A还是*A它们的值实际上是相同的,所以这样也是可以的:
MATRIX_INIT(AA, (float *)A, 2, 2);