fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'mathlib_new' into vector_control2

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-25 18:41:23 +04:00
parent 1e4bb764a6 69218d2bd5
commit 26daae0b0a
45 changed files with 1355 additions and 3043 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Dcm.cpp
-179
View File
@@ -1,179 +0,0 @@
/****************************************************************************
*
* 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 <mathlib/math/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(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[3][3]) :
Matrix(3, 3)
{
Dcm &dcm = *this;
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
dcm(i, j) = data[i][j];
}
Dcm::Dcm(const float *data) :
Matrix(3, 3, data)
{
}
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(const Matrix &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
src/lib/mathlib/math/Dcm.hpp
-113
View File
@@ -1,113 +0,0 @@
/****************************************************************************
*
* 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);
/**
* array ctor
*/
Dcm(const float data[3][3]);
/**
* quaternion ctor
*/
Dcm(const Quaternion &q);
/**
* euler angles ctor
*/
Dcm(const EulerAngles &euler);
/**
* copy ctor (deep)
*/
Dcm(const Dcm &right);
/**
* copy ctor (deep)
*/
Dcm(const Matrix &right);
/**
* dtor
*/
virtual ~Dcm();
};
int __EXPORT dcmTest();
} // math
src/lib/mathlib/math/EulerAngles.cpp
-126
View File
@@ -1,126 +0,0 @@
/****************************************************************************
*
* 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
src/lib/mathlib/math/EulerAngles.hpp
-74
View File
@@ -1,74 +0,0 @@
/****************************************************************************
*
* 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
src/lib/mathlib/math/Matrix.cpp
-193
View File
@@ -1,193 +0,0 @@
/****************************************************************************
*
* 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 <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
src/lib/mathlib/math/Matrix.hpp
+395 -21
View File
@@ -1,6 +1,9 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,30 +35,401 @@
****************************************************************************/
/**
* @file Matrix.h
* @file Matrix.hpp
*
* matrix code
* Matrix class
*/
#pragma once
#ifndef MATRIX_HPP
#define MATRIX_HPP
#include <nuttx/config.h>
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
#include "arm/Matrix.hpp"
#else
#include "generic/Matrix.hpp"
#endif
#include <stdio.h>
#include "../CMSIS/Include/arm_math.h"
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
template <unsigned int M, unsigned int N>
class __EXPORT Matrix;
// MxN matrix with float elements
template <unsigned int M, unsigned int N>
class __EXPORT MatrixBase
{
public:
/**
* matrix data[row][col]
*/
float data[M][N];
/**
* struct for using arm_math functions
*/
arm_matrix_instance_f32 arm_mat;
/**
* trivial ctor
* note that this ctor will not initialize elements
*/
MatrixBase() {
arm_mat = {M, N, &data[0][0]};
}
/**
* copyt ctor
*/
MatrixBase(const MatrixBase<M, N> &m) {
arm_mat = {M, N, &data[0][0]};
memcpy(data, m.data, sizeof(data));
}
MatrixBase(const float *d) {
arm_mat = {M, N, &data[0][0]};
memcpy(data, d, sizeof(data));
}
MatrixBase(const float d[M][N]) {
arm_mat = {M, N, &data[0][0]};
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float *d) {
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float d[M][N]) {
memcpy(data, d, sizeof(data));
}
/**
* access by index
*/
float &operator()(const unsigned int row, const unsigned int col) {
return data[row][col];
}
/**
* access by index
*/
float operator()(const unsigned int row, const unsigned int col) const {
return data[row][col];
}
/**
* get rows number
*/
unsigned int get_rows() const {
return M;
}
/**
* get columns number
*/
unsigned int get_cols() const {
return N;
}
/**
* test for equality
*/
bool operator ==(const Matrix<M, N> &m) const {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m.data[i][j])
return false;
return true;
}
/**
* test for inequality
*/
bool operator !=(const Matrix<M, N> &m) const {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m.data[i][j])
return true;
return false;
}
/**
* set to value
*/
const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
memcpy(data, m.data, sizeof(data));
return *static_cast<Matrix<M, N>*>(this);
}
/**
* negation
*/
Matrix<M, N> operator -(void) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
res.data[i][j] = -data[i][j];
return res;
}
/**
* addition
*/
Matrix<M, N> operator +(const Matrix<M, N> &m) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
res.data[i][j] = data[i][j] + m.data[i][j];
return res;
}
Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
data[i][j] += m.data[i][j];
return *static_cast<Matrix<M, N>*>(this);
}
/**
* subtraction
*/
Matrix<M, N> operator -(const Matrix<M, N> &m) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res.data[i][j] = data[i][j] - m.data[i][j];
return res;
}
Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
data[i][j] -= m.data[i][j];
return *static_cast<Matrix<M, N>*>(this);
}
/**
* uniform scaling
*/
Matrix<M, N> operator *(const float num) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res.data[i][j] = data[i][j] * num;
return res;
}
Matrix<M, N> &operator *=(const float num) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
data[i][j] *= num;
return *static_cast<Matrix<M, N>*>(this);
}
Matrix<M, N> operator /(const float num) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res[i][j] = data[i][j] / num;
return res;
}
Matrix<M, N> &operator /=(const float num) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
data[i][j] /= num;
return *static_cast<Matrix<M, N>*>(this);
}
/**
* multiplication by another matrix
*/
template <unsigned int P>
Matrix<M, P> operator *(const Matrix<N, P> &m) const {
Matrix<M, P> res;
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
return res;
}
/**
* transpose the matrix
*/
Matrix<N, M> transposed(void) const {
Matrix<N, M> res;
arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
return res;
}
/**
* invert the matrix
*/
Matrix<M, N> inversed(void) const {
Matrix<M, N> res;
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
}
/**
* set zero matrix
*/
void zero(void) {
memset(data, 0, sizeof(data));
}
/**
* set identity matrix
*/
void identity(void) {
memset(data, 0, sizeof(data));
unsigned int n = (M < N) ? M : N;
for (unsigned int i = 0; i < n; i++)
data[i][i] = 1;
}
void print(void) {
for (unsigned int i = 0; i < M; i++) {
printf("[ ");
for (unsigned int j = 0; j < N; j++)
printf("%.3f\t", data[i][j]);
printf(" ]\n");
}
}
};
template <unsigned int M, unsigned int N>
class __EXPORT Matrix : public MatrixBase<M, N>
{
public:
using MatrixBase<M, N>::operator *;
Matrix() : MatrixBase<M, N>() {}
Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}
Matrix(const float *d) : MatrixBase<M, N>(d) {}
Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {}
/**
* set to value
*/
const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
memcpy(this->data, m.data, sizeof(this->data));
return *this;
}
/**
* multiplication by a vector
*/
Vector<M> operator *(const Vector<N> &v) const {
Vector<M> res;
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
return res;
}
};
template <>
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
{
public:
using MatrixBase<3, 3>::operator *;
Matrix() : MatrixBase<3, 3>() {}
Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}
Matrix(const float *d) : MatrixBase<3, 3>(d) {}
Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {}
/**
* set to value
*/
const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) {
memcpy(this->data, m.data, sizeof(this->data));
return *this;
}
/**
* multiplication by a vector
*/
Vector<3> operator *(const Vector<3> &v) const {
Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
return res;
}
/**
* create a rotation matrix from given euler angles
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
*/
void from_euler(float roll, float pitch, float yaw) {
float cp = cosf(pitch);
float sp = sinf(pitch);
float sr = sinf(roll);
float cr = cosf(roll);
float sy = sinf(yaw);
float cy = cosf(yaw);
data[0][0] = cp * cy;
data[0][1] = (sr * sp * cy) - (cr * sy);
data[0][2] = (cr * sp * cy) + (sr * sy);
data[1][0] = cp * sy;
data[1][1] = (sr * sp * sy) + (cr * cy);
data[1][2] = (cr * sp * sy) - (sr * cy);
data[2][0] = -sp;
data[2][1] = sr * cp;
data[2][2] = cr * cp;
}
/**
* get euler angles from rotation matrix
*/
Vector<3> to_euler(void) const {
Vector<3> euler;
euler.data[1] = asinf(-data[2][0]);
if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];
} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];
} else {
euler.data[0] = atan2f(data[2][1], data[2][2]);
euler.data[2] = atan2f(data[1][0], data[0][0]);
}
return euler;
}
};
}
#endif // MATRIX_HPP
src/lib/mathlib/math/Quaternion.cpp
-174
View File
@@ -1,174 +0,0 @@
/****************************************************************************
*
* 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
src/lib/mathlib/math/Quaternion.hpp
+94 -44
View File
@@ -1,6 +1,9 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,82 +37,129 @@
/**
* @file Quaternion.hpp
*
* math quaternion lib
* Quaternion class
*/
#pragma once
#ifndef QUATERNION_HPP
#define QUATERNION_HPP
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
#include "Vector.hpp"
#include "Matrix.hpp"
namespace math
{
class Dcm;
class EulerAngles;
class __EXPORT Quaternion : public Vector
class __EXPORT Quaternion : public Vector<4>
{
public:
/**
* trivial ctor
*/
Quaternion() : Vector<4>() {}
/**
* default ctor
* copy ctor
*/
Quaternion();
Quaternion(const Quaternion &q) : Vector<4>(q) {}
/**
* ctor from floats
* casting from vector
*/
Quaternion(float a, float b, float c, float d);
Quaternion(const Vector<4> &v) : Vector<4>(v) {}
/**
* ctor from data
* setting ctor
*/
Quaternion(const float *data);
Quaternion(const float d[4]) : Vector<4>(d) {}
/**
* ctor from Vector
* setting ctor
*/
Quaternion(const Vector &v);
Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {}
using Vector<4>::operator *;
/**
* ctor from EulerAngles
* multiplication
*/
Quaternion(const EulerAngles &euler);
/**
* ctor from Dcm
*/
Quaternion(const Dcm &dcm);
/**
* deep copy ctor
*/
Quaternion(const Quaternion &right);
/**
* dtor
*/
virtual ~Quaternion();
const Quaternion operator *(const Quaternion &q) const {
return Quaternion(
data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
}
/**
* derivative
*/
Vector derivative(const Vector &w);
const Quaternion derivative(const Vector<3> &w) {
float dataQ[] = {
data[0], -data[1], -data[2], -data[3],
data[1], data[0], -data[3], data[2],
data[2], data[3], data[0], -data[1],
data[3], -data[2], data[1], data[0]
};
Matrix<4, 4> Q(dataQ);
Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
return Q * v * 0.5f;
}
/**
* accessors
* imaginary part of quaternion
*/
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); }
Vector<3> imag(void) {
return Vector<3>(&data[1]);
}
/**
* set quaternion to rotation defined by euler angles
*/
void from_euler(float roll, float pitch, float yaw) {
double cosPhi_2 = cos(double(roll) / 2.0);
double sinPhi_2 = sin(double(roll) / 2.0);
double cosTheta_2 = cos(double(pitch) / 2.0);
double sinTheta_2 = sin(double(pitch) / 2.0);
double cosPsi_2 = cos(double(yaw) / 2.0);
double sinPsi_2 = sin(double(yaw) / 2.0);
data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2;
data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2;
data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
}
void from_dcm(const Matrix<3, 3> &m) {
// avoiding singularities by not using division equations
data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]);
data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]);
data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]);
data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]);
}
/**
* create rotation matrix for the quaternion
*/
Matrix<3, 3> to_dcm(void) const {
Matrix<3, 3> R;
float aSq = data[0] * data[0];
float bSq = data[1] * data[1];
float cSq = data[2] * data[2];
float dSq = data[3] * data[3];
R.data[0][0] = aSq + bSq - cSq - dSq;
R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
R.data[1][1] = aSq - bSq + cSq - dSq;
R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
R.data[2][2] = aSq - bSq - cSq + dSq;
return R;
}
};
int __EXPORT quaternionTest();
} // math
}
#endif // QUATERNION_HPP
src/lib/mathlib/math/Vector.cpp
-100
View File
@@ -1,100 +0,0 @@
/****************************************************************************
*
* 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
src/lib/mathlib/math/Vector.hpp
+436 -17
View File
@@ -1,6 +1,9 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,26 +35,442 @@
****************************************************************************/
/**
* @file Vector.h
* @file Vector.hpp
*
* math vector
* Vector class
*/
#pragma once
#ifndef VECTOR_HPP
#define VECTOR_HPP
#include <nuttx/config.h>
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
#include "arm/Vector.hpp"
#else
#include "generic/Vector.hpp"
#endif
#include <stdio.h>
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
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
template <unsigned int N>
class __EXPORT Vector;
template <unsigned int N>
class __EXPORT VectorBase
{
public:
/**
* vector data
*/
float data[N];
/**
* struct for using arm_math functions, represents column vector
*/
arm_matrix_instance_f32 arm_col;
/**
* trivial ctor
* note that this ctor will not initialize elements
*/
VectorBase() {
arm_col = {N, 1, &data[0]};
}
/**
* copy ctor
*/
VectorBase(const VectorBase<N> &v) {
arm_col = {N, 1, &data[0]};
memcpy(data, v.data, sizeof(data));
}
/**
* setting ctor
*/
VectorBase(const float d[N]) {
arm_col = {N, 1, &data[0]};
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float d[N]) {
memcpy(data, d, sizeof(data));
}
/**
* access to elements by index
*/
float &operator()(const unsigned int i) {
return data[i];
}
/**
* access to elements by index
*/
float operator()(const unsigned int i) const {
return data[i];
}
/**
* get vector size
*/
unsigned int get_size() const {
return N;
}
/**
* test for equality
*/
bool operator ==(const Vector<N> &v) const {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v.data[i])
return false;
return true;
}
/**
* test for inequality
*/
bool operator !=(const Vector<N> &v) const {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v.data[i])
return true;
return false;
}
/**
* set to value
*/
const Vector<N> &operator =(const Vector<N> &v) {
memcpy(data, v.data, sizeof(data));
return *static_cast<const Vector<N>*>(this);
}
/**
* negation
*/
const Vector<N> operator -(void) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = -data[i];
return res;
}
/**
* addition
*/
const Vector<N> operator +(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] + v.data[i];
return res;
}
/**
* subtraction
*/
const Vector<N> operator -(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] - v.data[i];
return res;
}
/**
* uniform scaling
*/
const Vector<N> operator *(const float num) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] * num;
return res;
}
/**
* uniform scaling
*/
const Vector<N> operator /(const float num) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] / num;
return res;
}
/**
* addition
*/
const Vector<N> &operator +=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
data[i] += v.data[i];
return *static_cast<const Vector<N>*>(this);
}
/**
* subtraction
*/
const Vector<N> &operator -=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
data[i] -= v.data[i];
return *static_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
const Vector<N> &operator *=(const float num) {
for (unsigned int i = 0; i < N; i++)
data[i] *= num;
return *static_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
const Vector<N> &operator /=(const float num) {
for (unsigned int i = 0; i < N; i++)
data[i] /= num;
return *static_cast<const Vector<N>*>(this);
}
/**
* dot product
*/
float operator *(const Vector<N> &v) const {
float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * v.data[i];
return res;
}
/**
* gets the length of this vector squared
*/
float length_squared() const {
float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * data[i];
return res;
}
/**
* gets the length of this vector
*/
float length() const {
float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * data[i];
return sqrtf(res);
}
/**
* normalizes this vector
*/
void normalize() {
*this /= length();
}
/**
* returns the normalized version of this vector
*/
Vector<N> normalized() const {
return *this / length();
}
/**
* set zero vector
*/
void zero(void) {
memset(data, 0, sizeof(data));
}
void print(void) {
printf("[ ");
for (unsigned int i = 0; i < N; i++)
printf("%.3f\t", data[i]);
printf("]\n");
}
};
template <unsigned int N>
class __EXPORT Vector : public VectorBase<N>
{
public:
Vector() : VectorBase<N>() {}
Vector(const Vector<N> &v) : VectorBase<N>(v) {}
Vector(const float d[N]) : VectorBase<N>(d) {}
/**
* set to value
*/
const Vector<N> &operator =(const Vector<N> &v) {
memcpy(this->data, v.data, sizeof(this->data));
return *this;
}
};
template <>
class __EXPORT Vector<2> : public VectorBase<2>
{
public:
Vector() : VectorBase<2>() {}
// simple copy is 1.6 times faster than memcpy
Vector(const Vector<2> &v) : VectorBase<2>() {
data[0] = v.data[0];
data[1] = v.data[1];
}
Vector(const float d[2]) : VectorBase<2>() {
data[0] = d[0];
data[1] = d[1];
}
Vector(const float x, const float y) : VectorBase<2>() {
data[0] = x;
data[1] = y;
}
/**
* set data
*/
void set(const float d[2]) {
data[0] = d[0];
data[1] = d[1];
}
/**
* set to value
*/
const Vector<2> &operator =(const Vector<2> &v) {
data[0] = v.data[0];
data[1] = v.data[1];
return *this;
}
float operator %(const Vector<2> &v) const {
return data[0] * v.data[1] - data[1] * v.data[0];
}
};
template <>
class __EXPORT Vector<3> : public VectorBase<3>
{
public:
Vector() : VectorBase<3>() {}
// simple copy is 1.6 times faster than memcpy
Vector(const Vector<3> &v) : VectorBase<3>() {
for (unsigned int i = 0; i < 3; i++)
data[i] = v.data[i];
}
Vector(const float d[3]) : VectorBase<3>() {
for (unsigned int i = 0; i < 3; i++)
data[i] = d[i];
}
Vector(const float x, const float y, const float z) : VectorBase<3>() {
data[0] = x;
data[1] = y;
data[2] = z;
}
/**
* set data
*/
void set(const float d[3]) {
for (unsigned int i = 0; i < 3; i++)
data[i] = d[i];
}
/**
* set to value
*/
const Vector<3> &operator =(const Vector<3> &v) {
for (unsigned int i = 0; i < 3; i++)
data[i] = v.data[i];
return *this;
}
Vector<3> operator %(const Vector<3> &v) const {
return Vector<3>(
data[1] * v.data[2] - data[2] * v.data[1],
data[2] * v.data[0] - data[0] * v.data[2],
data[0] * v.data[1] - data[1] * v.data[0]
);
}
};
template <>
class __EXPORT Vector<4> : public VectorBase<4>
{
public:
Vector() : VectorBase() {}
Vector(const Vector<4> &v) : VectorBase<4>() {
for (unsigned int i = 0; i < 4; i++)
data[i] = v.data[i];
}
Vector(const float d[4]) : VectorBase<4>() {
for (unsigned int i = 0; i < 4; i++)
data[i] = d[i];
}
Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() {
data[0] = x0;
data[1] = x1;
data[2] = x2;
data[3] = x3;
}
/**
* set data
*/
void set(const float d[4]) {
for (unsigned int i = 0; i < 4; i++)
data[i] = d[i];
}
/**
* set to value
*/
const Vector<4> &operator =(const Vector<4> &v) {
for (unsigned int i = 0; i < 4; i++)
data[i] = v.data[i];
return *this;
}
};
}
#endif // VECTOR_HPP
src/lib/mathlib/math/Vector2f.cpp
-103
View File
@@ -1,103 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Vector2f.cpp
*
* math vector
*/
#include "test/test.hpp"
#include "Vector2f.hpp"
namespace math
{
Vector2f::Vector2f() :
Vector(2)
{
}
Vector2f::Vector2f(const Vector &right) :
Vector(right)
{
#ifdef VECTOR_ASSERT
ASSERT(right.getRows() == 2);
#endif
}
Vector2f::Vector2f(float x, float y) :
Vector(2)
{
setX(x);
setY(y);
}
Vector2f::Vector2f(const float *data) :
Vector(2, data)
{
}
Vector2f::~Vector2f()
{
}
float Vector2f::cross(const Vector2f &b) const
{
const Vector2f &a = *this;
return a(0)*b(1) - a(1)*b(0);
}
float Vector2f::operator %(const Vector2f &v) const
{
return cross(v);
}
float Vector2f::operator *(const Vector2f &v) const
{
return dot(v);
}
int __EXPORT vector2fTest()
{
printf("Test Vector2f\t\t: ");
// test float ctor
Vector2f v(1, 2);
ASSERT(equal(v(0), 1));
ASSERT(equal(v(1), 2));
printf("PASS\n");
return 0;
}
} // namespace math
src/lib/mathlib/math/Vector2f.hpp
-79
View File
@@ -1,79 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Vector2f.hpp
*
* math 3 vector
*/
#pragma once
#include "Vector.hpp"
namespace math
{
class __EXPORT Vector2f :
public Vector
{
public:
Vector2f();
Vector2f(const Vector &right);
Vector2f(float x, float y);
Vector2f(const float *data);
virtual ~Vector2f();
float cross(const Vector2f &b) const;
float operator %(const Vector2f &v) const;
float operator *(const Vector2f &v) const;
inline Vector2f operator*(const float &right) const {
return Vector::operator*(right);
}
/**
* accessors
*/
void setX(float x) { (*this)(0) = x; }
void setY(float y) { (*this)(1) = y; }
const float &getX() const { return (*this)(0); }
const float &getY() const { return (*this)(1); }
};
class __EXPORT Vector2 :
public Vector2f
{
};
int __EXPORT vector2fTest();
} // math
src/lib/mathlib/math/Vector3.cpp
-109
View File
@@ -1,109 +0,0 @@
/****************************************************************************
*
* 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) const
{
const Vector3 &a = *this;
Vector3 result;
result(0) = a(1) * b(2) - a(2) * b(1);
result(1) = a(2) * b(0) - a(0) * b(2);
result(2) = a(0) * b(1) - a(1) * b(0);
return result;
}
Vector3 Vector3::operator %(const Vector3 &v) const
{
return cross(v);
}
float Vector3::operator *(const Vector3 &v) const
{
return dot(v);
}
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
src/lib/mathlib/math/Vector3.hpp
-79
View File
@@ -1,79 +0,0 @@
/****************************************************************************
*
* 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) const;
Vector3 operator %(const Vector3 &v) const;
float operator *(const Vector3 &v) const;
using Vector::operator *;
/**
* 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); }
};
class __EXPORT Vector3f :
public Vector3
{
};
int __EXPORT vector3Test();
} // math
src/lib/mathlib/math/arm/Matrix.cpp
-40
View File
@@ -1,40 +0,0 @@
/****************************************************************************
*
* 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"
src/lib/mathlib/math/arm/Matrix.hpp
-292
View File
@@ -1,292 +0,0 @@
/****************************************************************************
*
* 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 "../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 exponent;
float num = (*this)(i, j);
float2SigExp(num, sig, exponent);
printf("%6.3fe%03d ", (double)sig, exponent);
}
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
src/lib/mathlib/math/arm/Vector.cpp
-40
View File
@@ -1,40 +0,0 @@
/****************************************************************************
*
* 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"
src/lib/mathlib/math/arm/Vector.hpp
-236
View File
@@ -1,236 +0,0 @@
/****************************************************************************
*
* 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 "../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 exponent;
float num = (*this)(i);
float2SigExp(num, sig, exponent);
printf("%6.3fe%03d ", (double)sig, exponent);
}
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*(const 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;
}
inline Vector operator-(void) const {
Vector result(getRows());
arm_negate_f32((float *)getData(),
result.getData(),
getRows());
return result;
}
// other functions
inline float dot(const Vector &right) const {
float result = 0;
arm_dot_prod_f32((float *)getData(),
(float *)right.getData(),
getRows(),
&result);
return result;
}
inline float norm() const {
return sqrtf(dot(*this));
}
inline float length() const {
return norm();
}
inline Vector unit() const {
return (*this) / norm();
}
inline Vector normalized() const {
return unit();
}
inline void normalize() {
(*this) = (*this) / norm();
}
inline static Vector zero(size_t rows) {
Vector result(rows);
// calloc returns zeroed memory
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
src/lib/mathlib/math/generic/Matrix.cpp
-40
View File
@@ -1,40 +0,0 @@
/****************************************************************************
*
* 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"
src/lib/mathlib/math/generic/Matrix.hpp
-437
View File
@@ -1,437 +0,0 @@
/****************************************************************************
*
* 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 "../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
src/lib/mathlib/math/generic/Vector.cpp
-40
View File
@@ -1,40 +0,0 @@
/****************************************************************************
*
* 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"
src/lib/mathlib/math/generic/Vector.hpp
-245
View File
@@ -1,245 +0,0 @@
/****************************************************************************
*
* 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 "../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;
}
inline Vector operator-(void) const {
Vector result(getRows());
for (size_t i = 0; i < getRows(); i++) {
result(i) = -((*this)(i));
}
return result;
}
// other functions
inline float dot(const Vector &right) const {
float result = 0;
for (size_t i = 0; i < getRows(); i++) {
result += (*this)(i) * (*this)(i);
}
return result;
}
inline float norm() const {
return sqrtf(dot(*this));
}
inline float length() const {
return norm();
}
inline Vector unit() const {
return (*this) / norm();
}
inline Vector normalized() const {
return unit();
}
inline void normalize() {
(*this) = (*this) / norm();
}
inline static Vector zero(size_t rows) {
Vector result(rows);
// calloc returns zeroed memory
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