diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mathlib/math/Dcm.cpp | 9 | ||||
-rw-r--r-- | src/modules/mathlib/math/Dcm.hpp | 5 | ||||
-rw-r--r-- | src/modules/mathlib/math/Limits.cpp | 130 | ||||
-rw-r--r-- | src/modules/mathlib/math/Limits.hpp | 80 | ||||
-rw-r--r-- | src/modules/mathlib/math/Quaternion.hpp | 2 | ||||
-rw-r--r-- | src/modules/mathlib/math/Vector2f.cpp | 103 | ||||
-rw-r--r-- | src/modules/mathlib/math/Vector2f.hpp | 79 | ||||
-rw-r--r-- | src/modules/mathlib/math/Vector3.cpp | 4 | ||||
-rw-r--r-- | src/modules/mathlib/math/Vector3.hpp | 7 | ||||
-rw-r--r-- | src/modules/mathlib/math/arm/Vector.hpp | 22 | ||||
-rw-r--r-- | src/modules/mathlib/math/generic/Vector.hpp | 24 | ||||
-rw-r--r-- | src/modules/mathlib/mathlib.h | 4 | ||||
-rw-r--r-- | src/modules/mathlib/module.mk | 4 |
13 files changed, 462 insertions, 11 deletions
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp index c3742e288..f509f7081 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 28d840b10..df8970d3a 100644 --- a/src/modules/mathlib/math/Dcm.hpp +++ b/src/modules/mathlib/math/Dcm.hpp @@ -77,6 +77,11 @@ public: Dcm(const float *data); /** + * array ctor + */ + Dcm(const float data[3][3]); + + /** * quaternion ctor */ Dcm(const Quaternion &q); diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp new file mode 100644 index 000000000..810a4484d --- /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 <math.h> + +#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 000000000..e1f2e7078 --- /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 <nuttx/config.h> + +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 4b4e959d8..048a55d33 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 000000000..68e741817 --- /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 000000000..ecd62e81c --- /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 61fcc442f..dcb85600e 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 8c36ac134..568d9669a 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 58d51107d..4155800e8 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 1a7363779..8cfdc676d 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 b919d53db..40ffb22bc 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 bcdb2afe5..2146a1413 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 |