然后现在发几段我认为有移植意义的代码。

#include "main.h"
#include <math.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>

float* vector_add(float result[3], int num, ...)
{
	//多个向量加
	va_list valist;
	va_start(valist, num);

	memset(result, 0, 12);
	 
	float *vec;
	for(uint8_t i = 0;i < num;i++)
	{
		vec = va_arg(valist, float*);
		for(uint8_t j = 0;j < 3;j++)result[j] += vec[j];
	}

	va_end(valist);

	return result;
}

float* scalar_mult(float result[3], float k, float vec[3])
{
	//数乘
	for(uint8_t i = 0;i < 3;i++)result[i] = k*vec[i];
	return result;
}

float* cross_product(float result[3], float vec1[3] ,float vec2[3])
{
	//叉乘
	result[0] = vec1[1]*vec2[2]-vec1[2]*vec2[1];
	result[1] = vec1[2]*vec2[0]-vec1[0]*vec2[2];
	result[2] = vec1[0]*vec2[1]-vec1[1]*vec2[0];
	return result;
}

float* left_mult(float result[3], float mat[3][3], float vec[3])
{
	//矩阵左乘
	result[0] = mat[0][0]*vec[0]+mat[0][1]*vec[1]+mat[0][2]*vec[2];
	result[1] = mat[1][0]*vec[0]+mat[1][1]*vec[1]+mat[1][2]*vec[2];
	result[2] = mat[2][0]*vec[0]+mat[2][1]*vec[1]+mat[2][2]*vec[2];
	return result;
}

来源INSS0工程中的matrix_count.c全部，这里主要是实现三维向量线性相加，数乘，叉乘，矩阵左乘的函数，这些在惯导解算中较为常用。线性相加用了stdarg.h的不定参数，用于实现多个向量加和。用法是外部定义结果存储的位置，然后参数和返回值都指向那个位置，方便嵌套，节省代码量且更接近于数学公式。