头文件command.h
#ifndef _COMMON_H_
#define _COMMON_H_
#define True 0
#define False (~True)
#define BIT0 (0x01)
#define BIT1 (BIT0<<1)
#define BIT2 (BIT0<<2)
#define BIT3 (BIT0<<3)
#define BIT4 (BIT0<<4)
#define BIT5 (BIT0<<5)
#define BIT6 (BIT0<<6)
#define BIT7 (BIT0<<7)
#define RET_SUCCESS 0x00
#define RET_FALSE 0xFF
#define DELAY_MS(x) HAL_Delay(x)
typedef struct Quaternion
{
float q0;
float q1;
float q2;
float q3;
}Quaternion;
typedef struct Vector_u16
{
short int x;
short int y;
short int z;
}Vector_u16;
typedef struct Vector_f
{
short int x;
short int y;
short int z;
}Vector_f;
typedef struct Mat_3x3_f
{
float data[3][3];
}Mat_3x3_f;
#endif
实现两个3×3矩阵相乘
Mat_3x3_f* Matrix_mult_3x3(const Mat_3x3_f* a, const Mat_3x3_f* b, Mat_3x3_f* out)
{
float sum;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
sum = 0;
for (int k = 0; k < 3; k++) {
sum += a->data[i][k] * b->data[k][j];
}
out->data[i][j] = sum;
}
}
return out;
}
实现3×3矩阵与3×1矩阵相称
Vector_f* Matrix_mult_3x1(const Mat_3x3_f* a, const Vector_f* b, Vector_f* out)
{
out->x = a->data[0][0] * b->x + a->data[0][1] * b->y + a->data[0][2] * b->z;
out->y = a->data[1][0] * b->x + a->data[1][1] * b->y + a->data[1][2] * b->z;
out->z = a->data[2][0] * b->x + a->data[2][1] * b->y + a->data[2][2] * b->z;
return out;
}
测试代码
//测试代码
int main() {
/*
//初始化
Mat_3x3_f a = { 1,0,0,0,1,0,0,0,1 };
Mat_3x3_f b = { 1,0,0,0,1,0,0,0,2 };
Mat_3x3_f c;
Vector_f d = { 1,2,3 };
Vector_f e;
//调用自定义函数
Matrix_mult_3x3(&a,&b,&c);
Matrix_mult_3x1(&a,&d,&e);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.2f ",c.data[i][j]);
}
}
printf("\n"); //换行
printf("%d %d %d",e.x,e.y,e.z);
*/
//验证3*3规格矩阵相乘的结果
Matrix3f matrix_1 = Eigen::Matrix3f::Random();
Matrix3f matrix_2 = Eigen::Matrix3f::Random();
matrix_1 = matrix_1.transpose();
matrix_2 = matrix_2.transpose();
Mat_3x3_f a;
Mat_3x3_f b;
Mat_3x3_f c;
printf("matrix_1=\n");
cout << matrix_1<<endl;
printf("----------------------------------------------------------------\n");
printf("matrix_2=\n");
cout << matrix_2<<endl;
printf("----------------------------------------------------------------\n");
//地址赋值
memcpy(&a,matrix_1.data(),sizeof(Mat_3x3_f));
memcpy(&b,matrix_2.data(),sizeof(Mat_3x3_f));
//输出a的值
printf("输出经过赋值后a的值=\n");
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.6f ", a.data[i][j]);
}
printf("\n");
}
printf("----------------------------------------------------------------\n");
//输出b的值
printf("输出经过赋值后b的值=\n");
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.6f ", b.data[i][j]);
}
printf("\n");
}
printf("----------------------------------------------------------------\n");
printf("通过Matrix_mult_3x3函数计算所得的结果=\n");
//矩阵相乘并求结果
Matrix_mult_3x3(&a, &b, &c);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.6f ", c.data[i][j]);
}
printf("\n");
}
printf("----------------------------------------------------------------\n");
printf("matrix_1 * matrix_2=\n");
cout << matrix_1 * matrix_2 << endl;
//验证3*3规格矩阵与3*1矩阵相乘后的结果
Vector_f d;
Vector_f e;
MatrixXf m1(3,1);
float a_1 = rand();
float b_1 = rand();
float c_1 = rand();
printf("----------------------------------------------------------------\n");
m1 << a_1, b_1, c_1;
//输出m1
printf("m1=\n");
cout << m1 << endl;
printf("----------------------------------------------------------------\n");
printf("经过赋值后d=");
d.x = a_1;
d.y = b_1;
d.z = c_1;
cout << d.x << endl;
cout << d.y << endl;
cout << d.z << endl;
printf("----------------------------------------------------------------\n");
printf("通过Matrix_mult_3x1函数计算所得的结果=\n");
Matrix_mult_3x1(&a, &d, &e);
cout << e.x << endl;
cout << e.y << endl;
cout << e.z << endl;
printf("----------------------------------------------------------------\n");
printf("matrix_1 * m1=\n");
cout << matrix_1 * m1 << endl;
}
Matrix_multiplication.cpp完整代码
#include "common.h"
#include <iostream>
#include <string.h>
#include "stdio.h"
#include "Eigen/Core"
#include <Eigen/Dense>
using namespace Eigen; //调用Eigen
using namespace std;
// TODO: 完成(3*3)矩阵乘法 函数返回输出指针
Mat_3x3_f* Matrix_mult_3x3(const Mat_3x3_f* a, const Mat_3x3_f* b, Mat_3x3_f* out);
// TODO: 完成(3*3)矩阵与(3x1)向量乘法
Vector_f* Matrix_mult_3x1(const Mat_3x3_f* a, const Vector_f* b, Vector_f* out);
// TODO: 完成(3*3)矩阵乘法 函数返回输出指针
Mat_3x3_f* Matrix_mult_3x3(const Mat_3x3_f* a, const Mat_3x3_f* b, Mat_3x3_f* out)
{
float sum;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
sum = 0;
for (int k = 0; k < 3; k++) {
sum += a->data[i][k] * b->data[k][j];
}
out->data[i][j] = sum;
}
}
return out;
}
// TODO: 完成(3*3)矩阵与(3x1)向量乘法
Vector_f* Matrix_mult_3x1(const Mat_3x3_f* a, const Vector_f* b, Vector_f* out)
{
out->x = a->data[0][0] * b->x + a->data[0][1] * b->y + a->data[0][2] * b->z;
out->y = a->data[1][0] * b->x + a->data[1][1] * b->y + a->data[1][2] * b->z;
out->z = a->data[2][0] * b->x + a->data[2][1] * b->y + a->data[2][2] * b->z;
return out;
}
//测试代码
int main() {
/*
//初始化
Mat_3x3_f a = { 1,0,0,0,1,0,0,0,1 };
Mat_3x3_f b = { 1,0,0,0,1,0,0,0,2 };
Mat_3x3_f c;
Vector_f d = { 1,2,3 };
Vector_f e;
//调用自定义函数
Matrix_mult_3x3(&a,&b,&c);
Matrix_mult_3x1(&a,&d,&e);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.2f ",c.data[i][j]);
}
}
printf("\n"); //换行
printf("%d %d %d",e.x,e.y,e.z);
*/
//验证3*3规格矩阵相乘的结果
Matrix3f matrix_1 = Eigen::Matrix3f::Random();
Matrix3f matrix_2 = Eigen::Matrix3f::Random();
matrix_1 = matrix_1.transpose();
matrix_2 = matrix_2.transpose();
Mat_3x3_f a;
Mat_3x3_f b;
Mat_3x3_f c;
printf("matrix_1=\n");
cout << matrix_1<<endl;
printf("----------------------------------------------------------------\n");
printf("matrix_2=\n");
cout << matrix_2<<endl;
printf("----------------------------------------------------------------\n");
//地址赋值
memcpy(&a,matrix_1.data(),sizeof(Mat_3x3_f));
memcpy(&b,matrix_2.data(),sizeof(Mat_3x3_f));
//输出a的值
printf("输出经过赋值后a的值=\n");
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.6f ", a.data[i][j]);
}
printf("\n");
}
printf("----------------------------------------------------------------\n");
//输出b的值
printf("输出经过赋值后b的值=\n");
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.6f ", b.data[i][j]);
}
printf("\n");
}
printf("----------------------------------------------------------------\n");
printf("通过Matrix_mult_3x3函数计算所得的结果=\n");
//矩阵相乘并求结果
Matrix_mult_3x3(&a, &b, &c);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%.6f ", c.data[i][j]);
}
printf("\n");
}
printf("----------------------------------------------------------------\n");
printf("matrix_1 * matrix_2=\n");
cout << matrix_1 * matrix_2 << endl;
//验证3*3规格矩阵与3*1矩阵相乘后的结果
Vector_f d;
Vector_f e;
MatrixXf m1(3,1);
float a_1 = rand();
float b_1 = rand();
float c_1 = rand();
printf("----------------------------------------------------------------\n");
m1 << a_1, b_1, c_1;
//输出m1
printf("m1=\n");
cout << m1 << endl;
printf("----------------------------------------------------------------\n");
printf("经过赋值后d=");
d.x = a_1;
d.y = b_1;
d.z = c_1;
cout << d.x << endl;
cout << d.y << endl;
cout << d.z << endl;
printf("----------------------------------------------------------------\n");
printf("通过Matrix_mult_3x1函数计算所得的结果=\n");
Matrix_mult_3x1(&a, &d, &e);
cout << e.x << endl;
cout << e.y << endl;
cout << e.z << endl;
printf("----------------------------------------------------------------\n");
printf("matrix_1 * m1=\n");
cout << matrix_1 * m1 << endl;
}
测试结果