Substantial improvements to math lib

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