aboutsummaryrefslogtreecommitdiff
path: root/src/lib/mathlib/math/Matrix.hpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib/mathlib/math/Matrix.hpp')
-rw-r--r--src/lib/mathlib/math/Matrix.hpp416
1 files changed, 395 insertions, 21 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index f19db15ec..ea0cf4ca1 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -1,6 +1,9 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.com>
+ * Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,30 +35,401 @@
****************************************************************************/
/**
- * @file Matrix.h
+ * @file Matrix.hpp
*
- * matrix code
+ * Matrix class
*/
-#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 <stdio.h>
+#include "../CMSIS/Include/arm_math.h"
namespace math
{
-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
+
+template <unsigned int M, unsigned int N>
+class __EXPORT Matrix;
+
+// MxN matrix with float elements
+template <unsigned int M, unsigned int N>
+class __EXPORT 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 = {M, N, &data[0][0]};
+ }
+
+ /**
+ * copyt ctor
+ */
+ MatrixBase(const MatrixBase<M, N> &m) {
+ arm_mat = {M, N, &data[0][0]};
+ memcpy(data, m.data, sizeof(data));
+ }
+
+ MatrixBase(const float *d) {
+ arm_mat = {M, N, &data[0][0]};
+ memcpy(data, d, sizeof(data));
+ }
+
+ MatrixBase(const float d[M][N]) {
+ arm_mat = {M, N, &data[0][0]};
+ memcpy(data, d, sizeof(data));
+ }
+
+ /**
+ * set data
+ */
+ void set(const float *d) {
+ memcpy(data, d, sizeof(data));
+ }
+
+ /**
+ * set data
+ */
+ void set(const float d[M][N]) {
+ memcpy(data, d, sizeof(data));
+ }
+
+ /**
+ * access by index
+ */
+ float &operator()(const unsigned int row, const unsigned int col) {
+ return data[row][col];
+ }
+
+ /**
+ * access by index
+ */
+ float operator()(const unsigned int row, const unsigned int col) const {
+ return data[row][col];
+ }
+
+ /**
+ * get rows number
+ */
+ unsigned int get_rows() const {
+ return M;
+ }
+
+ /**
+ * get columns number
+ */
+ unsigned int get_cols() const {
+ return N;
+ }
+
+ /**
+ * test for equality
+ */
+ bool operator ==(const Matrix<M, N> &m) const {
+ for (unsigned int i = 0; i < M; i++)
+ for (unsigned int j = 0; j < N; j++)
+ if (data[i][j] != m.data[i][j])
+ return false;
+
+ return true;
+ }
+
+ /**
+ * test for inequality
+ */
+ bool operator !=(const Matrix<M, N> &m) const {
+ for (unsigned int i = 0; i < M; i++)
+ for (unsigned int j = 0; j < N; j++)
+ if (data[i][j] != m.data[i][j])
+ return true;
+
+ return false;
+ }
+
+ /**
+ * set to value
+ */
+ const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
+ memcpy(data, m.data, sizeof(data));
+ return *static_cast<Matrix<M, N>*>(this);
+ }
+
+ /**
+ * negation
+ */
+ Matrix<M, N> operator -(void) const {
+ Matrix<M, N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ for (unsigned int j = 0; j < M; j++)
+ res.data[i][j] = -data[i][j];
+
+ return res;
+ }
+
+ /**
+ * addition
+ */
+ Matrix<M, N> operator +(const Matrix<M, N> &m) const {
+ Matrix<M, N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ for (unsigned int j = 0; j < M; j++)
+ res.data[i][j] = data[i][j] + m.data[i][j];
+
+ return res;
+ }
+
+ Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
+ for (unsigned int i = 0; i < N; i++)
+ for (unsigned int j = 0; j < M; j++)
+ data[i][j] += m.data[i][j];
+
+ return *static_cast<Matrix<M, N>*>(this);
+ }
+
+ /**
+ * subtraction
+ */
+ Matrix<M, N> operator -(const Matrix<M, N> &m) const {
+ Matrix<M, N> res;
+
+ for (unsigned int i = 0; i < M; i++)
+ for (unsigned int j = 0; j < N; j++)
+ res.data[i][j] = data[i][j] - m.data[i][j];
+
+ return res;
+ }
+
+ Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
+ for (unsigned int i = 0; i < N; i++)
+ for (unsigned int j = 0; j < M; j++)
+ data[i][j] -= m.data[i][j];
+
+ return *static_cast<Matrix<M, N>*>(this);
+ }
+
+ /**
+ * uniform scaling
+ */
+ Matrix<M, N> operator *(const float num) const {
+ Matrix<M, N> res;
+
+ for (unsigned int i = 0; i < M; i++)
+ for (unsigned int j = 0; j < N; j++)
+ res.data[i][j] = data[i][j] * num;
+
+ return res;
+ }
+
+ Matrix<M, N> &operator *=(const float num) {
+ for (unsigned int i = 0; i < M; i++)
+ for (unsigned int j = 0; j < N; j++)
+ data[i][j] *= num;
+
+ return *static_cast<Matrix<M, N>*>(this);
+ }
+
+ Matrix<M, N> operator /(const float num) const {
+ Matrix<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;
+ }
+
+ Matrix<M, N> &operator /=(const float num) {
+ for (unsigned int i = 0; i < M; i++)
+ for (unsigned int j = 0; j < N; j++)
+ data[i][j] /= num;
+
+ return *static_cast<Matrix<M, N>*>(this);
+ }
+
+ /**
+ * multiplication by another matrix
+ */
+ template <unsigned int P>
+ Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+ Matrix<M, P> res;
+ arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
+ return res;
+ }
+
+ /**
+ * transpose the matrix
+ */
+ Matrix<N, M> transposed(void) const {
+ Matrix<N, M> res;
+ arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
+ return res;
+ }
+
+ /**
+ * invert the matrix
+ */
+ Matrix<M, N> inversed(void) const {
+ Matrix<M, N> res;
+ arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
+ return res;
+ }
+
+ /**
+ * set zero matrix
+ */
+ void zero(void) {
+ memset(data, 0, sizeof(data));
+ }
+
+ /**
+ * set identity matrix
+ */
+ void identity(void) {
+ memset(data, 0, sizeof(data));
+ unsigned int n = (M < N) ? M : N;
+
+ for (unsigned int i = 0; i < n; i++)
+ data[i][i] = 1;
+ }
+
+ void print(void) {
+ for (unsigned int i = 0; i < M; i++) {
+ printf("[ ");
+
+ for (unsigned int j = 0; j < N; j++)
+ printf("%.3f\t", data[i][j]);
+
+ printf(" ]\n");
+ }
+ }
+};
+
+template <unsigned int M, unsigned int N>
+class __EXPORT Matrix : public MatrixBase<M, N>
+{
+public:
+ using MatrixBase<M, N>::operator *;
+
+ Matrix() : MatrixBase<M, N>() {}
+
+ Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}
+
+ Matrix(const float *d) : MatrixBase<M, N>(d) {}
+
+ Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {}
+
+ /**
+ * 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<M> 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 __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
+{
+public:
+ using MatrixBase<3, 3>::operator *;
+
+ Matrix() : MatrixBase<3, 3>() {}
+
+ Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}
+
+ Matrix(const float *d) : MatrixBase<3, 3>(d) {}
+
+ Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {}
+
+ /**
+ * 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(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
+ data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
+ data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
+ return res;
+ }
+
+ /**
+ * create a rotation matrix from given euler angles
+ * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
+ */
+ void from_euler(float roll, float pitch, float yaw) {
+ float cp = cosf(pitch);
+ float sp = sinf(pitch);
+ float sr = sinf(roll);
+ float cr = cosf(roll);
+ float sy = sinf(yaw);
+ float cy = cosf(yaw);
+
+ data[0][0] = cp * cy;
+ data[0][1] = (sr * sp * cy) - (cr * sy);
+ data[0][2] = (cr * sp * cy) + (sr * sy);
+ data[1][0] = cp * sy;
+ data[1][1] = (sr * sp * sy) + (cr * cy);
+ data[1][2] = (cr * sp * sy) - (sr * cy);
+ data[2][0] = -sp;
+ data[2][1] = sr * cp;
+ data[2][2] = cr * cp;
+ }
+
+ /**
+ * get euler angles from rotation matrix
+ */
+ Vector<3> to_euler(void) const {
+ Vector<3> euler;
+ euler.data[1] = asinf(-data[2][0]);
+
+ if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
+ euler.data[0] = 0.0f;
+ euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];
+
+ } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
+ euler.data[0] = 0.0f;
+ euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];
+
+ } else {
+ euler.data[0] = atan2f(data[2][1], data[2][2]);
+ euler.data[2] = atan2f(data[1][0], data[0][0]);
+ }
+
+ return euler;
+ }
+};
+
+}
+
+#endif // MATRIX_HPP
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-09 00:43:26 +0000