mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 23:53:34 +08:00
New mathlib, WIP
This commit is contained in:
Anton Babushkin
+233
-20
@@ -1,6 +1,8 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -32,30 +34,241 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Matrix.h
|
||||
* @file Matrix3.hpp
|
||||
*
|
||||
* matrix code
|
||||
* 3x3 Matrix
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef MATRIX_HPP
|
||||
#define MATRIX_HPP
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
|
||||
#include "arm/Matrix.hpp"
|
||||
#else
|
||||
#include "generic/Matrix.hpp"
|
||||
#endif
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <unsigned int N, unsigned int M>
|
||||
class Matrix;
|
||||
int matrixTest();
|
||||
int matrixAddTest();
|
||||
int matrixSubTest();
|
||||
int matrixMultTest();
|
||||
int matrixInvTest();
|
||||
int matrixDivTest();
|
||||
int matrixArmTest();
|
||||
bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
|
||||
} // namespace math
|
||||
|
||||
// MxN matrix with float elements
|
||||
template <unsigned int N, unsigned int M>
|
||||
class MatrixBase {
|
||||
public:
|
||||
/**
|
||||
* matrix data[row][col]
|
||||
*/
|
||||
float data[M][N];
|
||||
|
||||
/**
|
||||
* struct for using arm_math functions
|
||||
*/
|
||||
arm_matrix_instance_f32 arm_mat;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
* note that this ctor will not initialize elements
|
||||
*/
|
||||
MatrixBase() {
|
||||
//arm_mat_init_f32(&arm_mat, M, N, data);
|
||||
arm_mat = {M, N, &data[0][0]};
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
inline float &operator ()(unsigned int row, unsigned int col) {
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
inline const float &operator ()(unsigned int row, unsigned int col) const {
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
unsigned int getRows() {
|
||||
return M;
|
||||
}
|
||||
|
||||
unsigned int getCols() {
|
||||
return N;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const MatrixBase<M, N> &m) {
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const MatrixBase<M, N> &m) {
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const MatrixBase<M, N> &operator =(const MatrixBase<M, N> &m) {
|
||||
memcpy(data, m.data, sizeof(data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
MatrixBase<M, N> operator -(void) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
for (unsigned int j = 0; j < M; j++)
|
||||
res[i][j] = -data[i][j];
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
MatrixBase<M, N> operator +(const MatrixBase<M, N> &m) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
for (unsigned int j = 0; j < M; j++)
|
||||
res[i][j] = data[i][j] + m(i, j);
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator +=(const MatrixBase<M, N> &m) {
|
||||
return *this = *this + m;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
MatrixBase<M, N> operator -(const MatrixBase<M, N> &m) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] - m(i, j);
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator -=(const MatrixBase<M, N> &m) {
|
||||
return *this = *this - m;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
MatrixBase<M, N> operator *(const float num) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] * num;
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator *=(const float num) {
|
||||
return *this = *this * num;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> operator /(const float num) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] / num;
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator /=(const float num) {
|
||||
return *this = *this / num;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by another matrix
|
||||
*/
|
||||
template <unsigned int P>
|
||||
Matrix<N, P> operator *(const Matrix<M, P> &m) const {
|
||||
Matrix<N, P> res;
|
||||
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* setup the identity matrix
|
||||
*/
|
||||
void identity(void) {
|
||||
memset(data, 0, sizeof(data));
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
data[i][i] = 1;
|
||||
}
|
||||
|
||||
void dump(void) {
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
printf("%.3f\t", data[i][j]);
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template <unsigned int M, unsigned int N>
|
||||
class Matrix : public MatrixBase<M, N> {
|
||||
public:
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
|
||||
memcpy(this->data, m.data, sizeof(this->data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
/*
|
||||
Vector<N> operator *(const Vector<N> &v) const {
|
||||
Vector<M> res;
|
||||
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
|
||||
return res;
|
||||
}
|
||||
*/
|
||||
};
|
||||
|
||||
template <>
|
||||
class Matrix<3, 3> : public MatrixBase<3, 3> {
|
||||
public:
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) {
|
||||
memcpy(this->data, m.data, sizeof(this->data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
/*
|
||||
Vector<3> operator *(const Vector<3> &v) const {
|
||||
Vector<3> res;
|
||||
res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2);
|
||||
res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2);
|
||||
res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2);
|
||||
return res;
|
||||
}
|
||||
*/
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // MATRIX_HPP
|
||||
|
||||
Reference in New Issue
Block a user