diff options
Diffstat (limited to 'apps/systemlib/math')
-rw-r--r-- | apps/systemlib/math/Dcm.cpp | 136 | ||||
-rw-r--r-- | apps/systemlib/math/Dcm.hpp | 95 | ||||
-rw-r--r-- | apps/systemlib/math/EulerAngles.cpp | 124 | ||||
-rw-r--r-- | apps/systemlib/math/EulerAngles.hpp | 73 | ||||
-rw-r--r-- | apps/systemlib/math/Matrix.cpp | 180 | ||||
-rw-r--r-- | apps/systemlib/math/Matrix.hpp | 58 | ||||
-rw-r--r-- | apps/systemlib/math/Quaternion.cpp | 180 | ||||
-rw-r--r-- | apps/systemlib/math/Quaternion.hpp | 114 | ||||
-rw-r--r-- | apps/systemlib/math/Vector.cpp | 99 | ||||
-rw-r--r-- | apps/systemlib/math/Vector.hpp | 54 | ||||
-rw-r--r-- | apps/systemlib/math/Vector3.cpp | 99 | ||||
-rw-r--r-- | apps/systemlib/math/Vector3.hpp | 71 | ||||
-rw-r--r-- | apps/systemlib/math/arm/Matrix.cpp | 40 | ||||
-rw-r--r-- | apps/systemlib/math/arm/Matrix.hpp | 312 | ||||
-rw-r--r-- | apps/systemlib/math/arm/Vector.cpp | 40 | ||||
-rw-r--r-- | apps/systemlib/math/arm/Vector.hpp | 240 | ||||
-rw-r--r-- | apps/systemlib/math/generic/Matrix.cpp | 40 | ||||
-rw-r--r-- | apps/systemlib/math/generic/Matrix.hpp | 447 | ||||
-rw-r--r-- | apps/systemlib/math/generic/Vector.cpp | 40 | ||||
-rw-r--r-- | apps/systemlib/math/generic/Vector.hpp | 240 |
20 files changed, 2682 insertions, 0 deletions
diff --git a/apps/systemlib/math/Dcm.cpp b/apps/systemlib/math/Dcm.cpp new file mode 100644 index 000000000..1e9c4ee6d --- /dev/null +++ b/apps/systemlib/math/Dcm.cpp @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 <systemlib/test/test.hpp> + +#include "Dcm.hpp" +#include "Quaternion.hpp" +#include "EulerAngles.hpp" +#include "Vector3.hpp" + +namespace math +{ + +Dcm::Dcm() : + Matrix(Matrix::identity(3)) +{ +} + +Dcm::Dcm(const float * data) : + Matrix(3,3,data) +{ +} + +Dcm::Dcm(const Quaternion & q) : + Matrix(3,3) +{ + Dcm & dcm = *this; + float a = q.getA(); + float b = q.getB(); + float c = q.getC(); + float d = q.getD(); + float aSq = a*a; + float bSq = b*b; + float cSq = c*c; + float dSq = d*d; + dcm(0,0) = aSq + bSq - cSq - dSq; + dcm(0,1) = 2*(b*c - a*d); + dcm(0,2) = 2*(a*c + b*d); + dcm(1,0) = 2*(b*c + a*d); + dcm(1,1) = aSq - bSq + cSq - dSq; + dcm(1,2) = 2*(c*d - a*b); + dcm(2,0) = 2*(b*d - a*c); + dcm(2,1) = 2*(a*b + c*d); + dcm(2,2) = aSq - bSq - cSq + dSq; +} + +Dcm::Dcm(const EulerAngles & euler) : + Matrix(3,3) +{ + Dcm & dcm = *this; + float cosPhi = cosf(euler.getPhi()); + float sinPhi = sinf(euler.getPhi()); + float cosThe = cosf(euler.getTheta()); + float sinThe = sinf(euler.getTheta()); + float cosPsi = cosf(euler.getPsi()); + float sinPsi = sinf(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: "); + Vector3 vB(1,2,3); + ASSERT(matrixEqual(Dcm(Quaternion(1,0,0,0)), + Matrix::identity(3))); + ASSERT(matrixEqual(Dcm(EulerAngles(0,0,0)), + Matrix::identity(3))); + ASSERT(vectorEqual(Vector3(-2,1,3), + Dcm(EulerAngles(0,0,M_PI_2_F))*vB)); + ASSERT(vectorEqual(Vector3(3,2,-1), + Dcm(EulerAngles(0,M_PI_2_F,0))*vB)); + ASSERT(vectorEqual(Vector3(1,-3,2), + Dcm(EulerAngles(M_PI_2_F,0,0))*vB)); + ASSERT(vectorEqual(Vector3(3,2,-1), + 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/apps/systemlib/math/Dcm.hpp b/apps/systemlib/math/Dcm.hpp new file mode 100644 index 000000000..658d902fa --- /dev/null +++ b/apps/systemlib/math/Dcm.hpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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(); + + /** + * 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/apps/systemlib/math/EulerAngles.cpp b/apps/systemlib/math/EulerAngles.cpp new file mode 100644 index 000000000..b530f172d --- /dev/null +++ b/apps/systemlib/math/EulerAngles.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 <systemlib/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(1,2,3); + + // test ctor + ASSERT(vectorEqual(Vector3(1,2,3),euler)); + ASSERT(equal(euler.getPhi(),1)); + ASSERT(equal(euler.getTheta(),2)); + ASSERT(equal(euler.getPsi(),3)); + + // test dcm ctor + + // test assignment + euler.setPhi(4); + ASSERT(equal(euler.getPhi(),4)); + euler.setTheta(5); + ASSERT(equal(euler.getTheta(),5)); + euler.setPsi(6); + ASSERT(equal(euler.getPsi(),6)); + + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/apps/systemlib/math/EulerAngles.hpp b/apps/systemlib/math/EulerAngles.hpp new file mode 100644 index 000000000..59e5500ad --- /dev/null +++ b/apps/systemlib/math/EulerAngles.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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/apps/systemlib/math/Matrix.cpp b/apps/systemlib/math/Matrix.cpp new file mode 100644 index 000000000..08796da89 --- /dev/null +++ b/apps/systemlib/math/Matrix.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * 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 <systemlib/test/test.hpp> +#include <math.h> + +#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/apps/systemlib/math/Matrix.hpp b/apps/systemlib/math/Matrix.hpp new file mode 100644 index 000000000..efa71265c --- /dev/null +++ b/apps/systemlib/math/Matrix.hpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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 + +#ifdef ARM_MATH_CM4 +#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/apps/systemlib/math/Quaternion.cpp b/apps/systemlib/math/Quaternion.cpp new file mode 100644 index 000000000..79a913af6 --- /dev/null +++ b/apps/systemlib/math/Quaternion.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * 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 <systemlib/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) +{ + setA(0.5f*sqrtf(1 + dcm(0,0) + + dcm(1,1) + dcm(2,2))); + setB((dcm(2,1) - dcm(1,2))/ + (4*getA())); + setC((dcm(0,2) - dcm(2,0))/ + (4*getA())); + setD((dcm(1,0) - dcm(0,1))/ + (4*getA())); +} + +Quaternion::Quaternion(const EulerAngles & euler) : + Vector(4) +{ + float cosPhi_2 = cosf(euler.getPhi()/2.0f); + float cosTheta_2 = cosf(euler.getTheta()/2.0f); + float cosPsi_2 = cosf(euler.getPsi()/2.0f); + float sinPhi_2 = sinf(euler.getPhi()/2.0f); + float sinTheta_2 = sinf(euler.getTheta()/2.0f); + float sinPsi_2 = sinf(euler.getPsi()/2.0f); + 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)); + ASSERT(equal(q.getB(),0)); + ASSERT(equal(q.getC(),0)); + ASSERT(equal(q.getD(),0)); + // test float ctor + q = Quaternion(0,1,0,0); + ASSERT(equal(q.getA(),0)); + ASSERT(equal(q.getB(),1)); + ASSERT(equal(q.getC(),0)); + ASSERT(equal(q.getD(),0)); + // test euler ctor + q = Quaternion(EulerAngles(0,0,0)); + ASSERT(equal(q.getA(),1)); + ASSERT(equal(q.getB(),0)); + ASSERT(equal(q.getC(),0)); + ASSERT(equal(q.getD(),0)); + // test dcm ctor + q = Quaternion(Dcm()); + ASSERT(equal(q.getA(),1)); + ASSERT(equal(q.getB(),0)); + ASSERT(equal(q.getC(),0)); + ASSERT(equal(q.getD(),0)); + // TODO test derivative + // test accessors + q.setA(0.1); + q.setB(0.2); + q.setC(0.3); + q.setD(0.4); + ASSERT(equal(q.getA(),0.1)); + ASSERT(equal(q.getB(),0.2)); + ASSERT(equal(q.getC(),0.3)); + ASSERT(equal(q.getD(),0.4)); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/apps/systemlib/math/Quaternion.hpp b/apps/systemlib/math/Quaternion.hpp new file mode 100644 index 000000000..334b0dd03 --- /dev/null +++ b/apps/systemlib/math/Quaternion.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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/apps/systemlib/math/Vector.cpp b/apps/systemlib/math/Vector.cpp new file mode 100644 index 000000000..5bdbc9168 --- /dev/null +++ b/apps/systemlib/math/Vector.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 Vector.cpp + * + * math vector + */ + +#include <systemlib/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/apps/systemlib/math/Vector.hpp b/apps/systemlib/math/Vector.hpp new file mode 100644 index 000000000..0c5ac2b19 --- /dev/null +++ b/apps/systemlib/math/Vector.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 + +#ifdef ARM_MATH_CM4 +#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/apps/systemlib/math/Vector3.cpp b/apps/systemlib/math/Vector3.cpp new file mode 100644 index 000000000..a30b6d198 --- /dev/null +++ b/apps/systemlib/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 <systemlib/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/apps/systemlib/math/Vector3.hpp b/apps/systemlib/math/Vector3.hpp new file mode 100644 index 000000000..48dfd5b22 --- /dev/null +++ b/apps/systemlib/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/apps/systemlib/math/arm/Matrix.cpp b/apps/systemlib/math/arm/Matrix.cpp new file mode 100644 index 000000000..21661622a --- /dev/null +++ b/apps/systemlib/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/apps/systemlib/math/arm/Matrix.hpp b/apps/systemlib/math/arm/Matrix.hpp new file mode 100644 index 000000000..a94980872 --- /dev/null +++ b/apps/systemlib/math/arm/Matrix.hpp @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.h + * + * matrix code + */ + +#pragma once + + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include <systemlib/math/Vector.hpp> +#include <systemlib/math/Matrix.hpp> + +// arm specific +#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/apps/systemlib/math/arm/Vector.cpp b/apps/systemlib/math/arm/Vector.cpp new file mode 100644 index 000000000..7ea6496bb --- /dev/null +++ b/apps/systemlib/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/apps/systemlib/math/arm/Vector.hpp b/apps/systemlib/math/arm/Vector.hpp new file mode 100644 index 000000000..b12f112f6 --- /dev/null +++ b/apps/systemlib/math/arm/Vector.hpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.h + * + * math vector + */ + +#pragma once + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include <systemlib/math/Vector.hpp> +#include <systemlib/test/test.hpp> + +// arm specific +#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/apps/systemlib/math/generic/Matrix.cpp b/apps/systemlib/math/generic/Matrix.cpp new file mode 100644 index 000000000..21661622a --- /dev/null +++ b/apps/systemlib/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/apps/systemlib/math/generic/Matrix.hpp b/apps/systemlib/math/generic/Matrix.hpp new file mode 100644 index 000000000..fd6c8ba51 --- /dev/null +++ b/apps/systemlib/math/generic/Matrix.hpp @@ -0,0 +1,447 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.h + * + * matrix code + */ + +#pragma once + + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include <systemlib/math/Vector.hpp> +#include <systemlib/math/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/apps/systemlib/math/generic/Vector.cpp b/apps/systemlib/math/generic/Vector.cpp new file mode 100644 index 000000000..7ea6496bb --- /dev/null +++ b/apps/systemlib/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/apps/systemlib/math/generic/Vector.hpp b/apps/systemlib/math/generic/Vector.hpp new file mode 100644 index 000000000..4d867f360 --- /dev/null +++ b/apps/systemlib/math/generic/Vector.hpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.h + * + * math vector + */ + +#pragma once + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include <systemlib/math/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 |