mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
ce53ae63ae
MdB says: dividing by 2 is actually optimal. It's type correct for all usage, and the compiler generates the multiplication if it's float, and sticks with divide for integers Godbolt indicates on any optimization level (O1-O3, and Os) that the compiler will correctly optimize the / 2 into a float multiplication if using a float, but if using integer types in the template it will stick with the / 2 which is faster then doing the conversions to/from float.
61 lines
1.5 KiB
C++
61 lines
1.5 KiB
C++
/*
|
|
* N dimensional matrix operations
|
|
*/
|
|
|
|
#pragma GCC optimize("O3")
|
|
|
|
#include "matrixN.h"
|
|
|
|
|
|
// multiply two vectors to give a matrix, in-place
|
|
template <typename T, uint8_t N>
|
|
void MatrixN<T,N>::mult(const VectorN<T,N> &A, const VectorN<T,N> &B)
|
|
{
|
|
for (uint8_t i = 0; i < N; i++) {
|
|
for (uint8_t j = 0; j < N; j++) {
|
|
v[i][j] = A[i] * B[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
// subtract B from the matrix
|
|
template <typename T, uint8_t N>
|
|
MatrixN<T,N> &MatrixN<T,N>::operator -=(const MatrixN<T,N> &B)
|
|
{
|
|
for (uint8_t i = 0; i < N; i++) {
|
|
for (uint8_t j = 0; j < N; j++) {
|
|
v[i][j] -= B.v[i][j];
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
// add B to the matrix
|
|
template <typename T, uint8_t N>
|
|
MatrixN<T,N> &MatrixN<T,N>::operator +=(const MatrixN<T,N> &B)
|
|
{
|
|
for (uint8_t i = 0; i < N; i++) {
|
|
for (uint8_t j = 0; j < N; j++) {
|
|
v[i][j] += B.v[i][j];
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
// Matrix symmetry routine
|
|
template <typename T, uint8_t N>
|
|
void MatrixN<T,N>::force_symmetry(void)
|
|
{
|
|
for (uint8_t i = 0; i < N; i++) {
|
|
for (uint8_t j = 0; j < (i - 1); j++) {
|
|
v[i][j] = (v[i][j] + v[j][i]) / 2;
|
|
v[j][i] = v[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
template void MatrixN<float,4>::mult(const VectorN<float,4> &A, const VectorN<float,4> &B);
|
|
template MatrixN<float,4> &MatrixN<float,4>::operator -=(const MatrixN<float,4> &B);
|
|
template MatrixN<float,4> &MatrixN<float,4>::operator +=(const MatrixN<float,4> &B);
|
|
template void MatrixN<float,4>::force_symmetry(void);
|