aboutsummaryrefslogtreecommitdiff
path: root/src/lib/mathlib/math/arm
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib/mathlib/math/arm')
-rw-r--r--src/lib/mathlib/math/arm/Matrix.cpp40
-rw-r--r--src/lib/mathlib/math/arm/Matrix.hpp292
-rw-r--r--src/lib/mathlib/math/arm/Vector.cpp40
-rw-r--r--src/lib/mathlib/math/arm/Vector.hpp236
4 files changed, 608 insertions, 0 deletions
diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp
new file mode 100644
index 000000000..21661622a
--- /dev/null
+++ b/src/lib/mathlib/math/arm/Matrix.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "Matrix.hpp"
diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
new file mode 100644
index 000000000..715fd3a5e
--- /dev/null
+++ b/src/lib/mathlib/math/arm/Matrix.hpp
@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+#include "../Matrix.hpp"
+
+// arm specific
+#include "../../CMSIS/Include/arm_math.h"
+
+namespace math
+{
+
+class __EXPORT Matrix
+{
+public:
+ // constructor
+ Matrix(size_t rows, size_t cols) :
+ _matrix() {
+ arm_mat_init_f32(&_matrix,
+ rows, cols,
+ (float *)calloc(rows * cols, sizeof(float)));
+ }
+ Matrix(size_t rows, size_t cols, const float *data) :
+ _matrix() {
+ arm_mat_init_f32(&_matrix,
+ rows, cols,
+ (float *)malloc(rows * cols * sizeof(float)));
+ memcpy(getData(), data, getSize());
+ }
+ // deconstructor
+ virtual ~Matrix() {
+ delete [] _matrix.pData;
+ }
+ // copy constructor (deep)
+ Matrix(const Matrix &right) :
+ _matrix() {
+ arm_mat_init_f32(&_matrix,
+ right.getRows(), right.getCols(),
+ (float *)malloc(right.getRows()*
+ right.getCols()*sizeof(float)));
+ memcpy(getData(), right.getData(),
+ getSize());
+ }
+ // assignment
+ inline Matrix &operator=(const Matrix &right) {
+#ifdef MATRIX_ASSERT
+ ASSERT(getRows() == right.getRows());
+ ASSERT(getCols() == right.getCols());
+#endif
+
+ if (this != &right) {
+ memcpy(getData(), right.getData(),
+ right.getSize());
+ }
+
+ return *this;
+ }
+ // element accessors
+ inline float &operator()(size_t i, size_t j) {
+#ifdef MATRIX_ASSERT
+ ASSERT(i < getRows());
+ ASSERT(j < getCols());
+#endif
+ return getData()[i * getCols() + j];
+ }
+ inline const float &operator()(size_t i, size_t j) const {
+#ifdef MATRIX_ASSERT
+ ASSERT(i < getRows());
+ ASSERT(j < getCols());
+#endif
+ return getData()[i * getCols() + j];
+ }
+ // output
+ inline void print() const {
+ for (size_t i = 0; i < getRows(); i++) {
+ for (size_t j = 0; j < getCols(); j++) {
+ float sig;
+ int exp;
+ float num = (*this)(i, j);
+ float2SigExp(num, sig, exp);
+ printf("%6.3fe%03.3d,", (double)sig, exp);
+ }
+
+ printf("\n");
+ }
+ }
+ // boolean ops
+ inline bool operator==(const Matrix &right) const {
+ for (size_t i = 0; i < getRows(); i++) {
+ for (size_t j = 0; j < getCols(); j++) {
+ if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
+ return false;
+ }
+ }
+
+ return true;
+ }
+ // scalar ops
+ inline Matrix operator+(float right) const {
+ Matrix result(getRows(), getCols());
+ arm_offset_f32((float *)getData(), right,
+ (float *)result.getData(), getRows()*getCols());
+ return result;
+ }
+ inline Matrix operator-(float right) const {
+ Matrix result(getRows(), getCols());
+ arm_offset_f32((float *)getData(), -right,
+ (float *)result.getData(), getRows()*getCols());
+ return result;
+ }
+ inline Matrix operator*(float right) const {
+ Matrix result(getRows(), getCols());
+ arm_mat_scale_f32(&_matrix, right,
+ &(result._matrix));
+ return result;
+ }
+ inline Matrix operator/(float right) const {
+ Matrix result(getRows(), getCols());
+ arm_mat_scale_f32(&_matrix, 1.0f / right,
+ &(result._matrix));
+ return result;
+ }
+ // vector ops
+ inline Vector operator*(const Vector &right) const {
+#ifdef MATRIX_ASSERT
+ ASSERT(getCols() == right.getRows());
+#endif
+ Matrix resultMat = (*this) *
+ Matrix(right.getRows(), 1, right.getData());
+ return Vector(getRows(), resultMat.getData());
+ }
+ // matrix ops
+ inline Matrix operator+(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+ ASSERT(getRows() == right.getRows());
+ ASSERT(getCols() == right.getCols());
+#endif
+ Matrix result(getRows(), getCols());
+ arm_mat_add_f32(&_matrix, &(right._matrix),
+ &(result._matrix));
+ return result;
+ }
+ inline Matrix operator-(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+ ASSERT(getRows() == right.getRows());
+ ASSERT(getCols() == right.getCols());
+#endif
+ Matrix result(getRows(), getCols());
+ arm_mat_sub_f32(&_matrix, &(right._matrix),
+ &(result._matrix));
+ return result;
+ }
+ inline Matrix operator*(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+ ASSERT(getCols() == right.getRows());
+#endif
+ Matrix result(getRows(), right.getCols());
+ arm_mat_mult_f32(&_matrix, &(right._matrix),
+ &(result._matrix));
+ return result;
+ }
+ inline Matrix operator/(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+ ASSERT(right.getRows() == right.getCols());
+ ASSERT(getCols() == right.getCols());
+#endif
+ return (*this) * right.inverse();
+ }
+ // other functions
+ inline Matrix transpose() const {
+ Matrix result(getCols(), getRows());
+ arm_mat_trans_f32(&_matrix, &(result._matrix));
+ return result;
+ }
+ inline void swapRows(size_t a, size_t b) {
+ if (a == b) return;
+
+ for (size_t j = 0; j < getCols(); j++) {
+ float tmp = (*this)(a, j);
+ (*this)(a, j) = (*this)(b, j);
+ (*this)(b, j) = tmp;
+ }
+ }
+ inline void swapCols(size_t a, size_t b) {
+ if (a == b) return;
+
+ for (size_t i = 0; i < getRows(); i++) {
+ float tmp = (*this)(i, a);
+ (*this)(i, a) = (*this)(i, b);
+ (*this)(i, b) = tmp;
+ }
+ }
+ /**
+ * inverse based on LU factorization with partial pivotting
+ */
+ Matrix inverse() const {
+#ifdef MATRIX_ASSERT
+ ASSERT(getRows() == getCols());
+#endif
+ Matrix result(getRows(), getCols());
+ Matrix work = (*this);
+ arm_mat_inverse_f32(&(work._matrix),
+ &(result._matrix));
+ return result;
+ }
+ inline void setAll(const float &val) {
+ for (size_t i = 0; i < getRows(); i++) {
+ for (size_t j = 0; j < getCols(); j++) {
+ (*this)(i, j) = val;
+ }
+ }
+ }
+ inline void set(const float *data) {
+ memcpy(getData(), data, getSize());
+ }
+ inline size_t getRows() const { return _matrix.numRows; }
+ inline size_t getCols() const { return _matrix.numCols; }
+ inline static Matrix identity(size_t size) {
+ Matrix result(size, size);
+
+ for (size_t i = 0; i < size; i++) {
+ result(i, i) = 1.0f;
+ }
+
+ return result;
+ }
+ inline static Matrix zero(size_t size) {
+ Matrix result(size, size);
+ result.setAll(0.0f);
+ return result;
+ }
+ inline static Matrix zero(size_t m, size_t n) {
+ Matrix result(m, n);
+ result.setAll(0.0f);
+ return result;
+ }
+protected:
+ inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
+ inline float *getData() { return _matrix.pData; }
+ inline const float *getData() const { return _matrix.pData; }
+ inline void setData(float *data) { _matrix.pData = data; }
+private:
+ arm_matrix_instance_f32 _matrix;
+};
+
+} // namespace math
diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp
new file mode 100644
index 000000000..7ea6496bb
--- /dev/null
+++ b/src/lib/mathlib/math/arm/Vector.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include "Vector.hpp"
diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
new file mode 100644
index 000000000..4155800e8
--- /dev/null
+++ b/src/lib/mathlib/math/arm/Vector.hpp
@@ -0,0 +1,236 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+#include "../test/test.hpp"
+
+// arm specific
+#include "../../CMSIS/Include/arm_math.h"
+
+namespace math
+{
+
+class __EXPORT Vector
+{
+public:
+ // constructor
+ Vector(size_t rows) :
+ _rows(rows),
+ _data((float *)calloc(rows, sizeof(float))) {
+ }
+ Vector(size_t rows, const float *data) :
+ _rows(rows),
+ _data((float *)malloc(getSize())) {
+ memcpy(getData(), data, getSize());
+ }
+ // deconstructor
+ virtual ~Vector() {
+ delete [] getData();
+ }
+ // copy constructor (deep)
+ Vector(const Vector &right) :
+ _rows(right.getRows()),
+ _data((float *)malloc(getSize())) {
+ memcpy(getData(), right.getData(),
+ right.getSize());
+ }
+ // assignment
+ inline Vector &operator=(const Vector &right) {
+#ifdef VECTOR_ASSERT
+ ASSERT(getRows() == right.getRows());
+#endif
+
+ if (this != &right) {
+ memcpy(getData(), right.getData(),
+ right.getSize());
+ }
+
+ return *this;
+ }
+ // element accessors
+ inline float &operator()(size_t i) {
+#ifdef VECTOR_ASSERT
+ ASSERT(i < getRows());
+#endif
+ return getData()[i];
+ }
+ inline const float &operator()(size_t i) const {
+#ifdef VECTOR_ASSERT
+ ASSERT(i < getRows());
+#endif
+ return getData()[i];
+ }
+ // output
+ inline void print() const {
+ for (size_t i = 0; i < getRows(); i++) {
+ float sig;
+ int exp;
+ float num = (*this)(i);
+ float2SigExp(num, sig, exp);
+ printf("%6.3fe%03.3d,", (double)sig, exp);
+ }
+
+ printf("\n");
+ }
+ // boolean ops
+ inline bool operator==(const Vector &right) const {
+ for (size_t i = 0; i < getRows(); i++) {
+ if (fabsf(((*this)(i) - right(i))) > 1e-30f)
+ return false;
+ }
+
+ return true;
+ }
+ // scalar ops
+ inline Vector operator+(float right) const {
+ Vector result(getRows());
+ arm_offset_f32((float *)getData(),
+ right, result.getData(),
+ getRows());
+ return result;
+ }
+ inline Vector operator-(float right) const {
+ Vector result(getRows());
+ arm_offset_f32((float *)getData(),
+ -right, result.getData(),
+ getRows());
+ return result;
+ }
+ inline Vector operator*(float right) const {
+ Vector result(getRows());
+ arm_scale_f32((float *)getData(),
+ right, result.getData(),
+ getRows());
+ return result;
+ }
+ inline Vector operator/(float right) const {
+ Vector result(getRows());
+ arm_scale_f32((float *)getData(),
+ 1.0f / right, result.getData(),
+ getRows());
+ return result;
+ }
+ // vector ops
+ inline Vector operator+(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+ ASSERT(getRows() == right.getRows());
+#endif
+ Vector result(getRows());
+ arm_add_f32((float *)getData(),
+ (float *)right.getData(),
+ result.getData(),
+ getRows());
+ return result;
+ }
+ inline Vector operator-(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+ ASSERT(getRows() == right.getRows());
+#endif
+ Vector result(getRows());
+ arm_sub_f32((float *)getData(),
+ (float *)right.getData(),
+ result.getData(),
+ getRows());
+ return result;
+ }
+ inline Vector operator-(void) const {
+ Vector result(getRows());
+ arm_negate_f32((float *)getData(),
+ result.getData(),
+ getRows());
+ return result;
+ }
+ // other functions
+ inline float dot(const Vector &right) const {
+ float result = 0;
+ arm_dot_prod_f32((float *)getData(),
+ (float *)right.getData(),
+ getRows(),
+ &result);
+ return result;
+ }
+ inline float norm() const {
+ return sqrtf(dot(*this));
+ }
+ inline float length() const {
+ return norm();
+ }
+ inline Vector unit() const {
+ return (*this) / norm();
+ }
+ inline Vector normalized() const {
+ return unit();
+ }
+ inline void normalize() {
+ (*this) = (*this) / norm();
+ }
+ inline static Vector zero(size_t rows) {
+ Vector result(rows);
+ // calloc returns zeroed memory
+ return result;
+ }
+ inline void setAll(const float &val) {
+ for (size_t i = 0; i < getRows(); i++) {
+ (*this)(i) = val;
+ }
+ }
+ inline void set(const float *data) {
+ memcpy(getData(), data, getSize());
+ }
+ inline size_t getRows() const { return _rows; }
+ inline const float *getData() const { return _data; }
+protected:
+ inline size_t getSize() const { return sizeof(float) * getRows(); }
+ inline float *getData() { return _data; }
+ inline void setData(float *data) { _data = data; }
+private:
+ size_t _rows;
+ float *_data;
+};
+
+} // math
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-30 11:06:40 +0000