My code as follows, and in the main function, I recall Mat_product
function about 223440
times, use 179ns
, 23%
in the whole runtime.
struct Matrix_SE3 {
float R[3][3];
double P[3]; //here i need use double type.
};
struct Matrix_SE3 Mat_product(struct Matrix_SE3 A, struct Matrix_SE3 B) {
struct Matrix_SE3 result = { { { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } }, { 0,
0, 0 } };
for (int i = 0; i < 3; i++) {
result.P[i] += A.P[i];
for (int j = 0; j < 3; j++) {
result.P[i] += A.R[i][j] * B.P[j];
for (int k = 0; k < 3; k++)
result.R[i][j] += A.R[i][k] * B.R[k][j];
}
}
return result;
}
where $R$ is the rotation matrix, and $P$ represent the position, the function is calculated at two special euclidean group $SE(3)$ matrix multiplication and return $SE(3)$ matrix.
Maybe this is a duplicate of Optimized matrix multiplication in C, the difference is my code use struct
to describe matrix, does it affect the efficiency of calculation?