From f42b3ecd962a355081d36a62924e8ae9ecc05639 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jul 2013 15:43:38 +0200 Subject: [PATCH] Substantial improvements to math lib --- src/modules/mathlib/math/Dcm.cpp | 9 ++ src/modules/mathlib/math/Dcm.hpp | 5 + src/modules/mathlib/math/Limits.cpp | 130 ++++++++++++++++++++ src/modules/mathlib/math/Limits.hpp | 80 ++++++++++++ src/modules/mathlib/math/Quaternion.hpp | 2 +- src/modules/mathlib/math/Vector2f.cpp | 103 ++++++++++++++++ src/modules/mathlib/math/Vector2f.hpp | 79 ++++++++++++ src/modules/mathlib/math/Vector3.cpp | 4 +- src/modules/mathlib/math/Vector3.hpp | 7 +- src/modules/mathlib/math/arm/Vector.hpp | 22 +++- src/modules/mathlib/math/generic/Vector.hpp | 24 +++- src/modules/mathlib/mathlib.h | 4 + src/modules/mathlib/module.mk | 4 +- 13 files changed, 462 insertions(+), 11 deletions(-) create mode 100644 src/modules/mathlib/math/Limits.cpp create mode 100644 src/modules/mathlib/math/Limits.hpp create mode 100644 src/modules/mathlib/math/Vector2f.cpp create mode 100644 src/modules/mathlib/math/Vector2f.hpp diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp index c3742e2886..f509f7081b 100644 --- a/src/modules/mathlib/math/Dcm.cpp +++ b/src/modules/mathlib/math/Dcm.cpp @@ -69,6 +69,15 @@ Dcm::Dcm(float c00, float c01, float c02, dcm(2, 2) = c22; } +Dcm::Dcm(const float data[3][3]) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + dcm(i, j) = data[i][j]; +} + Dcm::Dcm(const float *data) : Matrix(3, 3, data) { diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp index 28d840b100..df8970d3aa 100644 --- a/src/modules/mathlib/math/Dcm.hpp +++ b/src/modules/mathlib/math/Dcm.hpp @@ -76,6 +76,11 @@ public: */ Dcm(const float *data); + /** + * array ctor + */ + Dcm(const float data[3][3]); + /** * quaternion ctor */ diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp new file mode 100644 index 0000000000..810a4484dc --- /dev/null +++ b/src/modules/mathlib/math/Limits.cpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 Limits.cpp + * + * Limiting / constrain helper functions + */ + + +#include + +#include "Limits.hpp" + + +namespace math { + + +float __EXPORT min(float val1, float val2) +{ + return (val1 < val2) ? val1 : val2; +} + +int __EXPORT min(int val1, int val2) +{ + return (val1 < val2) ? val1 : val2; +} + +unsigned __EXPORT min(unsigned val1, unsigned val2) +{ + return (val1 < val2) ? val1 : val2; +} + +double __EXPORT min(double val1, double val2) +{ + return (val1 < val2) ? val1 : val2; +} + +float __EXPORT max(float val1, float val2) +{ + return (val1 > val2) ? val1 : val2; +} + +int __EXPORT max(int val1, int val2) +{ + return (val1 > val2) ? val1 : val2; +} + +unsigned __EXPORT max(unsigned val1, unsigned val2) +{ + return (val1 > val2) ? val1 : val2; +} + +double __EXPORT max(double val1, double val2) +{ + return (val1 > val2) ? val1 : val2; +} + + +float __EXPORT constrain(float val, float min, float max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +int __EXPORT constrain(int val, int min, int max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +double __EXPORT constrain(double val, double min, double max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +float __EXPORT radians(float degrees) +{ + return (degrees / 180.0f) * M_PI_F; +} + +double __EXPORT radians(double degrees) +{ + return (degrees / 180.0) * M_PI; +} + +float __EXPORT degrees(float radians) +{ + return (radians / M_PI_F) * 180.0f; +} + +double __EXPORT degrees(double radians) +{ + return (radians / M_PI) * 180.0; +} + +} \ No newline at end of file diff --git a/src/modules/mathlib/math/Limits.hpp b/src/modules/mathlib/math/Limits.hpp new file mode 100644 index 0000000000..e1f2e70789 --- /dev/null +++ b/src/modules/mathlib/math/Limits.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 Limits.hpp + * + * Limiting / constrain helper functions + */ + +#pragma once + +#include + +namespace math { + + +float __EXPORT min(float val1, float val2); + +int __EXPORT min(int val1, int val2); + +unsigned __EXPORT min(unsigned val1, unsigned val2); + +double __EXPORT min(double val1, double val2); + +float __EXPORT max(float val1, float val2); + +int __EXPORT max(int val1, int val2); + +unsigned __EXPORT max(unsigned val1, unsigned val2); + +double __EXPORT max(double val1, double val2); + + +float __EXPORT constrain(float val, float min, float max); + +int __EXPORT constrain(int val, int min, int max); + +unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max); + +double __EXPORT constrain(double val, double min, double max); + +float __EXPORT radians(float degrees); + +double __EXPORT radians(double degrees); + +float __EXPORT degrees(float radians); + +double __EXPORT degrees(double radians); + +} \ No newline at end of file diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/mathlib/math/Quaternion.hpp index 4b4e959d8b..048a55d332 100644 --- a/src/modules/mathlib/math/Quaternion.hpp +++ b/src/modules/mathlib/math/Quaternion.hpp @@ -37,7 +37,7 @@ * math quaternion lib */ -//#pragma once +#pragma once #include "Vector.hpp" #include "Matrix.hpp" diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/modules/mathlib/math/Vector2f.cpp new file mode 100644 index 0000000000..68e741817d --- /dev/null +++ b/src/modules/mathlib/math/Vector2f.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 Vector2f.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector2f.hpp" + +namespace math +{ + +Vector2f::Vector2f() : + Vector(2) +{ +} + +Vector2f::Vector2f(const Vector &right) : + Vector(right) +{ +#ifdef VECTOR_ASSERT + ASSERT(right.getRows() == 2); +#endif +} + +Vector2f::Vector2f(float x, float y) : + Vector(2) +{ + setX(x); + setY(y); +} + +Vector2f::Vector2f(const float *data) : + Vector(2, data) +{ +} + +Vector2f::~Vector2f() +{ +} + +float Vector2f::cross(const Vector2f &b) const +{ + const Vector2f &a = *this; + return a(0)*b(1) - a(1)*b(0); +} + +float Vector2f::operator %(const Vector2f &v) const +{ + return cross(v); +} + +float Vector2f::operator *(const Vector2f &v) const +{ + return dot(v); +} + +int __EXPORT vector2fTest() +{ + printf("Test Vector2f\t\t: "); + // test float ctor + Vector2f v(1, 2); + ASSERT(equal(v(0), 1)); + ASSERT(equal(v(1), 2)); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/modules/mathlib/math/Vector2f.hpp new file mode 100644 index 0000000000..ecd62e81cb --- /dev/null +++ b/src/modules/mathlib/math/Vector2f.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 Vector2f.hpp + * + * math 3 vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class __EXPORT Vector2f : + public Vector +{ +public: + Vector2f(); + Vector2f(const Vector &right); + Vector2f(float x, float y); + Vector2f(const float *data); + virtual ~Vector2f(); + float cross(const Vector2f &b) const; + float operator %(const Vector2f &v) const; + float operator *(const Vector2f &v) const; + inline Vector2f operator*(const float &right) const { + return Vector::operator*(right); + } + + /** + * accessors + */ + void setX(float x) { (*this)(0) = x; } + void setY(float y) { (*this)(1) = y; } + const float &getX() const { return (*this)(0); } + const float &getY() const { return (*this)(1); } +}; + +class __EXPORT Vector2 : + public Vector2f +{ +}; + +int __EXPORT vector2fTest(); +} // math + diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp index 61fcc442f3..dcb85600ef 100644 --- a/src/modules/mathlib/math/Vector3.cpp +++ b/src/modules/mathlib/math/Vector3.cpp @@ -74,9 +74,9 @@ Vector3::~Vector3() { } -Vector3 Vector3::cross(const Vector3 &b) +Vector3 Vector3::cross(const Vector3 &b) const { - Vector3 &a = *this; + const Vector3 &a = *this; Vector3 result; result(0) = a(1) * b(2) - a(2) * b(1); result(1) = a(2) * b(0) - a(0) * b(2); diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp index 8c36ac1347..568d9669ab 100644 --- a/src/modules/mathlib/math/Vector3.hpp +++ b/src/modules/mathlib/math/Vector3.hpp @@ -53,7 +53,7 @@ public: Vector3(float x, float y, float z); Vector3(const float *data); virtual ~Vector3(); - Vector3 cross(const Vector3 &b); + Vector3 cross(const Vector3 &b) const; /** * accessors @@ -65,6 +65,11 @@ public: const float &getY() const { return (*this)(1); } const float &getZ() const { return (*this)(2); } }; + +class __EXPORT Vector3f : + public Vector3 +{ +}; int __EXPORT vector3Test(); } // math diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp index 58d51107d5..4155800e86 100644 --- a/src/modules/mathlib/math/arm/Vector.hpp +++ b/src/modules/mathlib/math/arm/Vector.hpp @@ -178,8 +178,15 @@ public: 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) { + inline float dot(const Vector &right) const { float result = 0; arm_dot_prod_f32((float *)getData(), (float *)right.getData(), @@ -187,12 +194,21 @@ public: &result); return result; } - inline float norm() { + inline float norm() const { return sqrtf(dot(*this)); } - inline Vector unit() { + 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 diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp index 1a73637798..8cfdc676d7 100644 --- a/src/modules/mathlib/math/generic/Vector.hpp +++ b/src/modules/mathlib/math/generic/Vector.hpp @@ -184,8 +184,17 @@ public: return result; } + inline Vector operator-(void) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = -((*this)(i)); + } + + return result; + } // other functions - inline float dot(const Vector &right) { + inline float dot(const Vector &right) const { float result = 0; for (size_t i = 0; i < getRows(); i++) { @@ -194,12 +203,21 @@ public: return result; } - inline float norm() { + inline float norm() const { return sqrtf(dot(*this)); } - inline Vector unit() { + 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 diff --git a/src/modules/mathlib/mathlib.h b/src/modules/mathlib/mathlib.h index b919d53dbb..40ffb22bc4 100644 --- a/src/modules/mathlib/mathlib.h +++ b/src/modules/mathlib/mathlib.h @@ -39,12 +39,16 @@ #ifdef __cplusplus +#pragma once + #include "math/Dcm.hpp" #include "math/EulerAngles.hpp" #include "math/Matrix.hpp" #include "math/Quaternion.hpp" #include "math/Vector.hpp" #include "math/Vector3.hpp" +#include "math/Vector2f.hpp" +#include "math/Limits.hpp" #endif diff --git a/src/modules/mathlib/module.mk b/src/modules/mathlib/module.mk index bcdb2afe50..2146a14131 100644 --- a/src/modules/mathlib/module.mk +++ b/src/modules/mathlib/module.mk @@ -36,11 +36,13 @@ # SRCS = math/test/test.cpp \ math/Vector.cpp \ + math/Vector2f.cpp \ math/Vector3.cpp \ math/EulerAngles.cpp \ math/Quaternion.cpp \ math/Dcm.cpp \ - math/Matrix.cpp + math/Matrix.cpp \ + math/Limits.cpp # # In order to include .config we first have to save off the