From 13fc6703862862f4263d8d5d085b7a16b87190e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 09:54:11 +0200 Subject: Moved last libs, drivers and headers, cleaned up IO build --- src/modules/mathlib/math/Dcm.cpp | 165 ++++++++++ src/modules/mathlib/math/Dcm.hpp | 103 ++++++ src/modules/mathlib/math/EulerAngles.cpp | 126 +++++++ src/modules/mathlib/math/EulerAngles.hpp | 74 +++++ src/modules/mathlib/math/Matrix.cpp | 193 +++++++++++ src/modules/mathlib/math/Matrix.hpp | 61 ++++ src/modules/mathlib/math/Quaternion.cpp | 174 ++++++++++ src/modules/mathlib/math/Quaternion.hpp | 115 +++++++ src/modules/mathlib/math/Vector.cpp | 100 ++++++ src/modules/mathlib/math/Vector.hpp | 57 ++++ src/modules/mathlib/math/Vector3.cpp | 99 ++++++ src/modules/mathlib/math/Vector3.hpp | 71 ++++ src/modules/mathlib/math/arm/Matrix.cpp | 40 +++ src/modules/mathlib/math/arm/Matrix.hpp | 292 +++++++++++++++++ src/modules/mathlib/math/arm/Vector.cpp | 40 +++ src/modules/mathlib/math/arm/Vector.hpp | 220 +++++++++++++ src/modules/mathlib/math/generic/Matrix.cpp | 40 +++ src/modules/mathlib/math/generic/Matrix.hpp | 437 +++++++++++++++++++++++++ src/modules/mathlib/math/generic/Vector.cpp | 40 +++ src/modules/mathlib/math/generic/Vector.hpp | 227 +++++++++++++ src/modules/mathlib/math/nasa_rotation_def.pdf | Bin 0 -> 709235 bytes src/modules/mathlib/math/test/test.cpp | 94 ++++++ src/modules/mathlib/math/test/test.hpp | 50 +++ src/modules/mathlib/math/test_math.sce | 63 ++++ 24 files changed, 2881 insertions(+) create mode 100644 src/modules/mathlib/math/Dcm.cpp create mode 100644 src/modules/mathlib/math/Dcm.hpp create mode 100644 src/modules/mathlib/math/EulerAngles.cpp create mode 100644 src/modules/mathlib/math/EulerAngles.hpp create mode 100644 src/modules/mathlib/math/Matrix.cpp create mode 100644 src/modules/mathlib/math/Matrix.hpp create mode 100644 src/modules/mathlib/math/Quaternion.cpp create mode 100644 src/modules/mathlib/math/Quaternion.hpp create mode 100644 src/modules/mathlib/math/Vector.cpp create mode 100644 src/modules/mathlib/math/Vector.hpp create mode 100644 src/modules/mathlib/math/Vector3.cpp create mode 100644 src/modules/mathlib/math/Vector3.hpp create mode 100644 src/modules/mathlib/math/arm/Matrix.cpp create mode 100644 src/modules/mathlib/math/arm/Matrix.hpp create mode 100644 src/modules/mathlib/math/arm/Vector.cpp create mode 100644 src/modules/mathlib/math/arm/Vector.hpp create mode 100644 src/modules/mathlib/math/generic/Matrix.cpp create mode 100644 src/modules/mathlib/math/generic/Matrix.hpp create mode 100644 src/modules/mathlib/math/generic/Vector.cpp create mode 100644 src/modules/mathlib/math/generic/Vector.hpp create mode 100644 src/modules/mathlib/math/nasa_rotation_def.pdf create mode 100644 src/modules/mathlib/math/test/test.cpp create mode 100644 src/modules/mathlib/math/test/test.hpp create mode 100644 src/modules/mathlib/math/test_math.sce (limited to 'src/modules/mathlib/math') diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp new file mode 100644 index 000000000..c3742e288 --- /dev/null +++ b/src/modules/mathlib/math/Dcm.cpp @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * 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 Dcm.cpp + * + * math direction cosine matrix + */ + +#include + +#include "Dcm.hpp" +#include "Quaternion.hpp" +#include "EulerAngles.hpp" +#include "Vector3.hpp" + +namespace math +{ + +Dcm::Dcm() : + Matrix(Matrix::identity(3)) +{ +} + +Dcm::Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + dcm(0, 0) = c00; + dcm(0, 1) = c01; + dcm(0, 2) = c02; + dcm(1, 0) = c10; + dcm(1, 1) = c11; + dcm(1, 2) = c12; + dcm(2, 0) = c20; + dcm(2, 1) = c21; + dcm(2, 2) = c22; +} + +Dcm::Dcm(const float *data) : + Matrix(3, 3, data) +{ +} + +Dcm::Dcm(const Quaternion &q) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + double a = q.getA(); + double b = q.getB(); + double c = q.getC(); + double d = q.getD(); + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + dcm(0, 0) = aSq + bSq - cSq - dSq; + dcm(0, 1) = 2.0 * (b * c - a * d); + dcm(0, 2) = 2.0 * (a * c + b * d); + dcm(1, 0) = 2.0 * (b * c + a * d); + dcm(1, 1) = aSq - bSq + cSq - dSq; + dcm(1, 2) = 2.0 * (c * d - a * b); + dcm(2, 0) = 2.0 * (b * d - a * c); + dcm(2, 1) = 2.0 * (a * b + c * d); + dcm(2, 2) = aSq - bSq - cSq + dSq; +} + +Dcm::Dcm(const EulerAngles &euler) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + double cosPhi = cos(euler.getPhi()); + double sinPhi = sin(euler.getPhi()); + double cosThe = cos(euler.getTheta()); + double sinThe = sin(euler.getTheta()); + double cosPsi = cos(euler.getPsi()); + double sinPsi = sin(euler.getPsi()); + + dcm(0, 0) = cosThe * cosPsi; + dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; + dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; + + dcm(1, 0) = cosThe * sinPsi; + dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; + dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; + + dcm(2, 0) = -sinThe; + dcm(2, 1) = sinPhi * cosThe; + dcm(2, 2) = cosPhi * cosThe; +} + +Dcm::Dcm(const Dcm &right) : + Matrix(right) +{ +} + +Dcm::~Dcm() +{ +} + +int __EXPORT dcmTest() +{ + printf("Test DCM\t\t: "); + // default ctor + ASSERT(matrixEqual(Dcm(), + Matrix::identity(3))); + // quaternion ctor + ASSERT(matrixEqual( + Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), + Dcm(0.9362934f, -0.2750958f, 0.2183507f, + 0.2896295f, 0.9564251f, -0.0369570f, + -0.1986693f, 0.0978434f, 0.9751703f))); + // euler angle ctor + ASSERT(matrixEqual( + Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), + Dcm(0.9362934f, -0.2750958f, 0.2183507f, + 0.2896295f, 0.9564251f, -0.0369570f, + -0.1986693f, 0.0978434f, 0.9751703f))); + // rotations + Vector3 vB(1, 2, 3); + ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), + Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); + ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), + Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); + ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), + Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); + ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), + Dcm(EulerAngles( + M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); + printf("PASS\n"); + return 0; +} +} // namespace math diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp new file mode 100644 index 000000000..28d840b10 --- /dev/null +++ b/src/modules/mathlib/math/Dcm.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 Dcm.hpp + * + * math direction cosine matrix + */ + +//#pragma once + +#include "Vector.hpp" +#include "Matrix.hpp" + +namespace math +{ + +class Quaternion; +class EulerAngles; + +/** + * This is a Tait Bryan, Body 3-2-1 sequence. + * (yaw)-(pitch)-(roll) + * The Dcm transforms a vector in the body frame + * to the navigation frame, typically represented + * as C_nb. C_bn can be obtained through use + * of the transpose() method. + */ +class __EXPORT Dcm : public Matrix +{ +public: + /** + * default ctor + */ + Dcm(); + + /** + * scalar ctor + */ + Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22); + + /** + * data ctor + */ + Dcm(const float *data); + + /** + * quaternion ctor + */ + Dcm(const Quaternion &q); + + /** + * euler angles ctor + */ + Dcm(const EulerAngles &euler); + + /** + * copy ctor (deep) + */ + Dcm(const Dcm &right); + + /** + * dtor + */ + virtual ~Dcm(); +}; + +int __EXPORT dcmTest(); + +} // math + diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/modules/mathlib/math/EulerAngles.cpp new file mode 100644 index 000000000..e733d23bb --- /dev/null +++ b/src/modules/mathlib/math/EulerAngles.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 "test/test.hpp" + +#include "EulerAngles.hpp" +#include "Quaternion.hpp" +#include "Dcm.hpp" +#include "Vector3.hpp" + +namespace math +{ + +EulerAngles::EulerAngles() : + Vector(3) +{ + setPhi(0.0f); + setTheta(0.0f); + setPsi(0.0f); +} + +EulerAngles::EulerAngles(float phi, float theta, float psi) : + Vector(3) +{ + setPhi(phi); + setTheta(theta); + setPsi(psi); +} + +EulerAngles::EulerAngles(const Quaternion &q) : + Vector(3) +{ + (*this) = EulerAngles(Dcm(q)); +} + +EulerAngles::EulerAngles(const Dcm &dcm) : + Vector(3) +{ + setTheta(asinf(-dcm(2, 0))); + + if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { + setPhi(0.0f); + setPsi(atan2f(dcm(1, 2) - dcm(0, 1), + dcm(0, 2) + dcm(1, 1)) + getPhi()); + + } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { + setPhi(0.0f); + setPsi(atan2f(dcm(1, 2) - dcm(0, 1), + dcm(0, 2) + dcm(1, 1)) - getPhi()); + + } else { + setPhi(atan2f(dcm(2, 1), dcm(2, 2))); + setPsi(atan2f(dcm(1, 0), dcm(0, 0))); + } +} + +EulerAngles::~EulerAngles() +{ +} + +int __EXPORT eulerAnglesTest() +{ + printf("Test EulerAngles\t: "); + EulerAngles euler(0.1f, 0.2f, 0.3f); + + // test ctor + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + ASSERT(equal(euler.getPhi(), 0.1f)); + ASSERT(equal(euler.getTheta(), 0.2f)); + ASSERT(equal(euler.getPsi(), 0.3f)); + + // test dcm ctor + euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + + // test quat ctor + euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + + // test assignment + euler.setPhi(0.4f); + euler.setTheta(0.5f); + euler.setPsi(0.6f); + ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); + + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/modules/mathlib/math/EulerAngles.hpp new file mode 100644 index 000000000..399eecfa7 --- /dev/null +++ b/src/modules/mathlib/math/EulerAngles.hpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 "Vector.hpp" + +namespace math +{ + +class Quaternion; +class Dcm; + +class __EXPORT EulerAngles : public Vector +{ +public: + EulerAngles(); + EulerAngles(float phi, float theta, float psi); + EulerAngles(const Quaternion &q); + EulerAngles(const Dcm &dcm); + virtual ~EulerAngles(); + + // alias + void setPhi(float phi) { (*this)(0) = phi; } + void setTheta(float theta) { (*this)(1) = theta; } + void setPsi(float psi) { (*this)(2) = psi; } + + // const accessors + const float &getPhi() const { return (*this)(0); } + const float &getTheta() const { return (*this)(1); } + const float &getPsi() const { return (*this)(2); } + +}; + +int __EXPORT eulerAnglesTest(); + +} // math + diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/modules/mathlib/math/Matrix.cpp new file mode 100644 index 000000000..ebd1aeda3 --- /dev/null +++ b/src/modules/mathlib/math/Matrix.cpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * 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 "test/test.hpp" +#include + +#include "Matrix.hpp" + +namespace math +{ + +static const float data_testA[] = { + 1, 2, 3, + 4, 5, 6 +}; +static Matrix testA(2, 3, data_testA); + +static const float data_testB[] = { + 0, 1, 3, + 7, -1, 2 +}; +static Matrix testB(2, 3, data_testB); + +static const float data_testC[] = { + 0, 1, + 2, 1, + 3, 2 +}; +static Matrix testC(3, 2, data_testC); + +static const float data_testD[] = { + 0, 1, 2, + 2, 1, 4, + 5, 2, 0 +}; +static Matrix testD(3, 3, data_testD); + +static const float data_testE[] = { + 1, -1, 2, + 0, 2, 3, + 2, -1, 1 +}; +static Matrix testE(3, 3, data_testE); + +static const float data_testF[] = { + 3.777e006f, 2.915e007f, 0.000e000f, + 2.938e007f, 2.267e008f, 0.000e000f, + 0.000e000f, 0.000e000f, 6.033e008f +}; +static Matrix testF(3, 3, data_testF); + +int __EXPORT matrixTest() +{ + matrixAddTest(); + matrixSubTest(); + matrixMultTest(); + matrixInvTest(); + matrixDivTest(); + return 0; +} + +int matrixAddTest() +{ + printf("Test Matrix Add\t\t: "); + Matrix r = testA + testB; + float data_test[] = { + 1.0f, 3.0f, 6.0f, + 11.0f, 4.0f, 8.0f + }; + ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixSubTest() +{ + printf("Test Matrix Sub\t\t: "); + Matrix r = testA - testB; + float data_test[] = { + 1.0f, 1.0f, 0.0f, + -3.0f, 6.0f, 4.0f + }; + ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixMultTest() +{ + printf("Test Matrix Mult\t: "); + Matrix r = testC * testB; + float data_test[] = { + 7.0f, -1.0f, 2.0f, + 7.0f, 1.0f, 8.0f, + 14.0f, 1.0f, 13.0f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixInvTest() +{ + printf("Test Matrix Inv\t\t: "); + Matrix origF = testF; + Matrix r = testF.inverse(); + float data_test[] = { + -0.0012518f, 0.0001610f, 0.0000000f, + 0.0001622f, -0.0000209f, 0.0000000f, + 0.0000000f, 0.0000000f, 1.6580e-9f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + // make sure F in unchanged + ASSERT(matrixEqual(origF, testF)); + printf("PASS\n"); + return 0; +} + +int matrixDivTest() +{ + printf("Test Matrix Div\t\t: "); + Matrix r = testD / testE; + float data_test[] = { + 0.2222222f, 0.5555556f, -0.1111111f, + 0.0f, 1.0f, 1.0, + -4.1111111f, 1.2222222f, 4.5555556f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +bool matrixEqual(const Matrix &a, const Matrix &b, float eps) +{ + if (a.getRows() != b.getRows()) { + printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); + return false; + + } else if (a.getCols() != b.getCols()) { + printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); + return false; + } + + bool ret = true; + + for (size_t i = 0; i < a.getRows(); i++) + for (size_t j = 0; j < a.getCols(); j++) { + if (!equal(a(i, j), b(i, j), eps)) { + printf("element mismatch (%d, %d)\n", i, j); + ret = false; + } + } + + return ret; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/modules/mathlib/math/Matrix.hpp new file mode 100644 index 000000000..f19db15ec --- /dev/null +++ b/src/modules/mathlib/math/Matrix.hpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 + +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) +#include "arm/Matrix.hpp" +#else +#include "generic/Matrix.hpp" +#endif + +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 diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/modules/mathlib/math/Quaternion.cpp new file mode 100644 index 000000000..02fec4ca6 --- /dev/null +++ b/src/modules/mathlib/math/Quaternion.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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 Quaternion.cpp + * + * math vector + */ + +#include "test/test.hpp" + + +#include "Quaternion.hpp" +#include "Dcm.hpp" +#include "EulerAngles.hpp" + +namespace math +{ + +Quaternion::Quaternion() : + Vector(4) +{ + setA(1.0f); + setB(0.0f); + setC(0.0f); + setD(0.0f); +} + +Quaternion::Quaternion(float a, float b, + float c, float d) : + Vector(4) +{ + setA(a); + setB(b); + setC(c); + setD(d); +} + +Quaternion::Quaternion(const float *data) : + Vector(4, data) +{ +} + +Quaternion::Quaternion(const Vector &v) : + Vector(v) +{ +} + +Quaternion::Quaternion(const Dcm &dcm) : + Vector(4) +{ + // avoiding singularities by not using + // division equations + setA(0.5 * sqrt(1.0 + + double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); + setB(0.5 * sqrt(1.0 + + double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); + setC(0.5 * sqrt(1.0 + + double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); + setD(0.5 * sqrt(1.0 + + double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); +} + +Quaternion::Quaternion(const EulerAngles &euler) : + Vector(4) +{ + double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); + double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); + double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); + double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); + double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); + double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); + setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); +} + +Quaternion::Quaternion(const Quaternion &right) : + Vector(right) +{ +} + +Quaternion::~Quaternion() +{ +} + +Vector Quaternion::derivative(const Vector &w) +{ +#ifdef QUATERNION_ASSERT + ASSERT(w.getRows() == 3); +#endif + float dataQ[] = { + getA(), -getB(), -getC(), -getD(), + getB(), getA(), -getD(), getC(), + getC(), getD(), getA(), -getB(), + getD(), -getC(), getB(), getA() + }; + Vector v(4); + v(0) = 0.0f; + v(1) = w(0); + v(2) = w(1); + v(3) = w(2); + Matrix Q(4, 4, dataQ); + return Q * v * 0.5f; +} + +int __EXPORT quaternionTest() +{ + printf("Test Quaternion\t\t: "); + // test default ctor + Quaternion q; + ASSERT(equal(q.getA(), 1.0f)); + ASSERT(equal(q.getB(), 0.0f)); + ASSERT(equal(q.getC(), 0.0f)); + ASSERT(equal(q.getD(), 0.0f)); + // test float ctor + q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); + ASSERT(equal(q.getA(), 0.1825742f)); + ASSERT(equal(q.getB(), 0.3651484f)); + ASSERT(equal(q.getC(), 0.5477226f)); + ASSERT(equal(q.getD(), 0.7302967f)); + // test euler ctor + q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); + // test dcm ctor + q = Quaternion(Dcm()); + ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); + // TODO test derivative + // test accessors + q.setA(0.1f); + q.setB(0.2f); + q.setC(0.3f); + q.setD(0.4f); + ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/mathlib/math/Quaternion.hpp new file mode 100644 index 000000000..4b4e959d8 --- /dev/null +++ b/src/modules/mathlib/math/Quaternion.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 Quaternion.hpp + * + * math quaternion lib + */ + +//#pragma once + +#include "Vector.hpp" +#include "Matrix.hpp" + +namespace math +{ + +class Dcm; +class EulerAngles; + +class __EXPORT Quaternion : public Vector +{ +public: + + /** + * default ctor + */ + Quaternion(); + + /** + * ctor from floats + */ + Quaternion(float a, float b, float c, float d); + + /** + * ctor from data + */ + Quaternion(const float *data); + + /** + * ctor from Vector + */ + Quaternion(const Vector &v); + + /** + * ctor from EulerAngles + */ + Quaternion(const EulerAngles &euler); + + /** + * ctor from Dcm + */ + Quaternion(const Dcm &dcm); + + /** + * deep copy ctor + */ + Quaternion(const Quaternion &right); + + /** + * dtor + */ + virtual ~Quaternion(); + + /** + * derivative + */ + Vector derivative(const Vector &w); + + /** + * accessors + */ + void setA(float a) { (*this)(0) = a; } + void setB(float b) { (*this)(1) = b; } + void setC(float c) { (*this)(2) = c; } + void setD(float d) { (*this)(3) = d; } + const float &getA() const { return (*this)(0); } + const float &getB() const { return (*this)(1); } + const float &getC() const { return (*this)(2); } + const float &getD() const { return (*this)(3); } +}; + +int __EXPORT quaternionTest(); +} // math + diff --git a/src/modules/mathlib/math/Vector.cpp b/src/modules/mathlib/math/Vector.cpp new file mode 100644 index 000000000..35158a396 --- /dev/null +++ b/src/modules/mathlib/math/Vector.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 "test/test.hpp" + +#include "Vector.hpp" + +namespace math +{ + +static const float data_testA[] = {1, 3}; +static const float data_testB[] = {4, 1}; + +static Vector testA(2, data_testA); +static Vector testB(2, data_testB); + +int __EXPORT vectorTest() +{ + vectorAddTest(); + vectorSubTest(); + return 0; +} + +int vectorAddTest() +{ + printf("Test Vector Add\t\t: "); + Vector r = testA + testB; + float data_test[] = {5.0f, 4.0f}; + ASSERT(vectorEqual(Vector(2, data_test), r)); + printf("PASS\n"); + return 0; +} + +int vectorSubTest() +{ + printf("Test Vector Sub\t\t: "); + Vector r(2); + r = testA - testB; + float data_test[] = { -3.0f, 2.0f}; + ASSERT(vectorEqual(Vector(2, data_test), r)); + printf("PASS\n"); + return 0; +} + +bool vectorEqual(const Vector &a, const Vector &b, float eps) +{ + if (a.getRows() != b.getRows()) { + printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); + return false; + } + + bool ret = true; + + for (size_t i = 0; i < a.getRows(); i++) { + if (!equal(a(i), b(i), eps)) { + printf("element mismatch (%d)\n", i); + ret = false; + } + } + + return ret; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Vector.hpp b/src/modules/mathlib/math/Vector.hpp new file mode 100644 index 000000000..73de793d5 --- /dev/null +++ b/src/modules/mathlib/math/Vector.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 + +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) +#include "arm/Vector.hpp" +#else +#include "generic/Vector.hpp" +#endif + +namespace math +{ +class Vector; +int __EXPORT vectorTest(); +int __EXPORT vectorAddTest(); +int __EXPORT vectorSubTest(); +bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); +} // math diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp new file mode 100644 index 000000000..61fcc442f --- /dev/null +++ b/src/modules/mathlib/math/Vector3.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 Vector3.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector3.hpp" + +namespace math +{ + +Vector3::Vector3() : + Vector(3) +{ +} + +Vector3::Vector3(const Vector &right) : + Vector(right) +{ +#ifdef VECTOR_ASSERT + ASSERT(right.getRows() == 3); +#endif +} + +Vector3::Vector3(float x, float y, float z) : + Vector(3) +{ + setX(x); + setY(y); + setZ(z); +} + +Vector3::Vector3(const float *data) : + Vector(3, data) +{ +} + +Vector3::~Vector3() +{ +} + +Vector3 Vector3::cross(const Vector3 &b) +{ + 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); + result(2) = a(0) * b(1) - a(1) * b(0); + return result; +} + +int __EXPORT vector3Test() +{ + printf("Test Vector3\t\t: "); + // test float ctor + Vector3 v(1, 2, 3); + ASSERT(equal(v(0), 1)); + ASSERT(equal(v(1), 2)); + ASSERT(equal(v(2), 3)); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp new file mode 100644 index 000000000..8c36ac134 --- /dev/null +++ b/src/modules/mathlib/math/Vector3.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 Vector3.hpp + * + * math 3 vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class __EXPORT Vector3 : + public Vector +{ +public: + Vector3(); + Vector3(const Vector &right); + Vector3(float x, float y, float z); + Vector3(const float *data); + virtual ~Vector3(); + Vector3 cross(const Vector3 &b); + + /** + * accessors + */ + void setX(float x) { (*this)(0) = x; } + void setY(float y) { (*this)(1) = y; } + void setZ(float z) { (*this)(2) = z; } + const float &getX() const { return (*this)(0); } + const float &getY() const { return (*this)(1); } + const float &getZ() const { return (*this)(2); } +}; + +int __EXPORT vector3Test(); +} // math + diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/modules/mathlib/math/arm/Matrix.cpp new file mode 100644 index 000000000..21661622a --- /dev/null +++ b/src/modules/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/modules/mathlib/math/arm/Matrix.hpp b/src/modules/mathlib/math/arm/Matrix.hpp new file mode 100644 index 000000000..715fd3a5e --- /dev/null +++ b/src/modules/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 +#include +#include +#include +#include +#include + +#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/modules/mathlib/math/arm/Vector.cpp b/src/modules/mathlib/math/arm/Vector.cpp new file mode 100644 index 000000000..7ea6496bb --- /dev/null +++ b/src/modules/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/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp new file mode 100644 index 000000000..58d51107d --- /dev/null +++ b/src/modules/mathlib/math/arm/Vector.hpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include + +#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; + } + // other functions + inline float dot(const Vector &right) { + float result = 0; + arm_dot_prod_f32((float *)getData(), + (float *)right.getData(), + getRows(), + &result); + return result; + } + inline float norm() { + return sqrtf(dot(*this)); + } + inline Vector unit() { + return (*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 diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/modules/mathlib/math/generic/Matrix.cpp new file mode 100644 index 000000000..21661622a --- /dev/null +++ b/src/modules/mathlib/math/generic/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/modules/mathlib/math/generic/Matrix.hpp b/src/modules/mathlib/math/generic/Matrix.hpp new file mode 100644 index 000000000..5601a3447 --- /dev/null +++ b/src/modules/mathlib/math/generic/Matrix.hpp @@ -0,0 +1,437 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include + +#include "../Vector.hpp" +#include "../Matrix.hpp" + +namespace math +{ + +class __EXPORT Matrix +{ +public: + // constructor + Matrix(size_t rows, size_t cols) : + _rows(rows), + _cols(cols), + _data((float *)calloc(rows *cols, sizeof(float))) { + } + Matrix(size_t rows, size_t cols, const float *data) : + _rows(rows), + _cols(cols), + _data((float *)malloc(getSize())) { + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Matrix() { + delete [] getData(); + } + // copy constructor (deep) + Matrix(const Matrix &right) : + _rows(right.getRows()), + _cols(right.getCols()), + _data((float *)malloc(getSize())) { + memcpy(getData(), right.getData(), + right.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+(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) + right; + } + } + + return result; + } + inline Matrix operator-(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) - right; + } + } + + return result; + } + inline Matrix operator*(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) * right; + } + } + + return result; + } + inline Matrix operator/(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) / right; + } + } + + return result; + } + // vector ops + inline Vector operator*(const Vector &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i) += (*this)(i, j) * right(j); + } + } + + return result; + } + // 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()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) + right(i, j); + } + } + + 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()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) - right(i, j); + } + } + + return result; + } + inline Matrix operator*(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Matrix result(getRows(), right.getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < right.getCols(); j++) { + for (size_t k = 0; k < right.getRows(); k++) { + result(i, j) += (*this)(i, k) * right(k, j); + } + } + } + + 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()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(j, i) = (*this)(i, j); + } + } + + 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 + size_t N = getRows(); + Matrix L = identity(N); + const Matrix &A = (*this); + Matrix U = A; + Matrix P = identity(N); + + //printf("A:\n"); A.print(); + + // for all diagonal elements + for (size_t n = 0; n < N; n++) { + + // if diagonal is zero, swap with row below + if (fabsf(U(n, n)) < 1e-8f) { + //printf("trying pivot for row %d\n",n); + for (size_t i = 0; i < N; i++) { + if (i == n) continue; + + //printf("\ttrying row %d\n",i); + if (fabsf(U(i, n)) > 1e-8f) { + //printf("swapped %d\n",i); + U.swapRows(i, n); + P.swapRows(i, n); + } + } + } + +#ifdef MATRIX_ASSERT + //printf("A:\n"); A.print(); + //printf("U:\n"); U.print(); + //printf("P:\n"); P.print(); + //fflush(stdout); + ASSERT(fabsf(U(n, n)) > 1e-8f); +#endif + + // failsafe, return zero matrix + if (fabsf(U(n, n)) < 1e-8f) { + return Matrix::zero(n); + } + + // for all rows below diagonal + for (size_t i = (n + 1); i < N; i++) { + L(i, n) = U(i, n) / U(n, n); + + // add i-th row and n-th row + // multiplied by: -a(i,n)/a(n,n) + for (size_t k = n; k < N; k++) { + U(i, k) -= L(i, n) * U(n, k); + } + } + } + + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); + + // solve LY=P*I for Y by forward subst + Matrix Y = P; + + // for all columns of Y + for (size_t c = 0; c < N; c++) { + // for all rows of L + for (size_t i = 0; i < N; i++) { + // for all columns of L + for (size_t j = 0; j < i; j++) { + // for all existing y + // subtract the component they + // contribute to the solution + Y(i, c) -= L(i, j) * Y(j, c); + } + + // divide by the factor + // on current + // term to be solved + // Y(i,c) /= L(i,i); + // but L(i,i) = 1.0 + } + } + + //printf("Y:\n"); Y.print(); + + // solve Ux=y for x by back subst + Matrix X = Y; + + // for all columns of X + for (size_t c = 0; c < N; c++) { + // for all rows of U + for (size_t k = 0; k < N; k++) { + // have to go in reverse order + size_t i = N - 1 - k; + + // for all columns of U + for (size_t j = i + 1; j < N; j++) { + // for all existing x + // subtract the component they + // contribute to the solution + X(i, c) -= U(i, j) * X(j, c); + } + + // divide by the factor + // on current + // term to be solved + X(i, c) /= U(i, i); + } + } + + //printf("X:\n"); X.print(); + return X; + } + 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 _rows; } + inline size_t getCols() const { return _cols; } + 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 _data; } + inline const float *getData() const { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + size_t _cols; + float *_data; +}; + +} // namespace math diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/modules/mathlib/math/generic/Vector.cpp new file mode 100644 index 000000000..7ea6496bb --- /dev/null +++ b/src/modules/mathlib/math/generic/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/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp new file mode 100644 index 000000000..1a7363779 --- /dev/null +++ b/src/modules/mathlib/math/generic/Vector.hpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include + +#include "../Vector.hpp" + +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+(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) + right; + } + + return result; + } + inline Vector operator-(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right; + } + + return result; + } + inline Vector operator*(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) * right; + } + + return result; + } + inline Vector operator/(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) / right; + } + + return result; + } + // vector ops + inline Vector operator+(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) + right(i); + } + + return result; + } + inline Vector operator-(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right(i); + } + + return result; + } + // other functions + inline float dot(const Vector &right) { + float result = 0; + + for (size_t i = 0; i < getRows(); i++) { + result += (*this)(i) * (*this)(i); + } + + return result; + } + inline float norm() { + return sqrtf(dot(*this)); + } + inline Vector unit() { + return (*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; } +protected: + inline size_t getSize() const { return sizeof(float) * getRows(); } + inline float *getData() { return _data; } + inline const float *getData() const { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + float *_data; +}; + +} // math diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/modules/mathlib/math/nasa_rotation_def.pdf new file mode 100644 index 000000000..eb67a4bfc Binary files /dev/null and b/src/modules/mathlib/math/nasa_rotation_def.pdf differ diff --git a/src/modules/mathlib/math/test/test.cpp b/src/modules/mathlib/math/test/test.cpp new file mode 100644 index 000000000..2fa2f7e7c --- /dev/null +++ b/src/modules/mathlib/math/test/test.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 test.cpp + * + * Test library code + */ + +#include +#include +#include + +#include "test.hpp" + +bool __EXPORT equal(float a, float b, float epsilon) +{ + float diff = fabsf(a - b); + + if (diff > epsilon) { + printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + + } else return true; +} + +void __EXPORT float2SigExp( + const float &num, + float &sig, + int &exp) +{ + if (isnan(num) || isinf(num)) { + sig = 0.0f; + exp = -99; + return; + } + + if (fabsf(num) < 1.0e-38f) { + sig = 0; + exp = 0; + return; + } + + exp = log10f(fabsf(num)); + + if (exp > 0) { + exp = ceil(exp); + + } else { + exp = floor(exp); + } + + sig = num; + + // cheap power since it is integer + if (exp > 0) { + for (int i = 0; i < abs(exp); i++) sig /= 10; + + } else { + for (int i = 0; i < abs(exp); i++) sig *= 10; + } +} + + diff --git a/src/modules/mathlib/math/test/test.hpp b/src/modules/mathlib/math/test/test.hpp new file mode 100644 index 000000000..2027bb827 --- /dev/null +++ b/src/modules/mathlib/math/test/test.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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 test.hpp + * + * Controller library code + */ + +#pragma once + +//#include +//#include +//#include + +bool equal(float a, float b, float eps = 1e-5); +void float2SigExp( + const float &num, + float &sig, + int &exp); diff --git a/src/modules/mathlib/math/test_math.sce b/src/modules/mathlib/math/test_math.sce new file mode 100644 index 000000000..c3fba4729 --- /dev/null +++ b/src/modules/mathlib/math/test_math.sce @@ -0,0 +1,63 @@ +clc +clear +function out = float_truncate(in, digits) + out = round(in*10^digits) + out = out/10^digits +endfunction + +phi = 0.1 +theta = 0.2 +psi = 0.3 + +cosPhi = cos(phi) +cosPhi_2 = cos(phi/2) +sinPhi = sin(phi) +sinPhi_2 = sin(phi/2) + +cosTheta = cos(theta) +cosTheta_2 = cos(theta/2) +sinTheta = sin(theta) +sinTheta_2 = sin(theta/2) + +cosPsi = cos(psi) +cosPsi_2 = cos(psi/2) +sinPsi = sin(psi) +sinPsi_2 = sin(psi/2) + +C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi; + cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi; + -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta] + +disp(C_nb) +//C_nb = float_truncate(C_nb,3) +//disp(C_nb) + +theta = asin(-C_nb(3,1)) +phi = atan(C_nb(3,2), C_nb(3,3)) +psi = atan(C_nb(2,1), C_nb(1,1)) +printf('phi %f\n', phi) +printf('theta %f\n', theta) +printf('psi %f\n', psi) + +q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2; + sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2; + cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2; + cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2] + +//q = float_truncate(q,3) + +a = q(1) +b = q(2) +c = q(3) +d = q(4) +printf('q: %f %f %f %f\n', a, b, c, d) +a2 = a*a +b2 = b*b +c2 = c*c +d2 = d*d + +C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c); + 2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b); + 2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2] + +disp(C2_nb) -- cgit v1.2.3