Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 22:24:47 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

Merge commit 'd9491b20cc5fc8b683eb0f60a50da6b322b55e57' into local/mathlib

Browse Source
This commit is contained in:
px4dev
2013-01-06 12:57:33 -08:00
parent 95aa959571 d9491b20cc
commit 98a53e85d6
19 changed files with 1432 additions and 1453 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/fix_code_style_ubuntu.sh
Executable
+19
View File
@@ -0,0 +1,19 @@
#!/bin/sh
astyle \
--style=linux \
--indent=force-tab=8 \
--indent-cases \
--indent-preprocessor \
--break-blocks=all \
--pad-oper \
--pad-header \
--unpad-paren \
--keep-one-line-blocks \
--keep-one-line-statements \
--align-pointer=name \
--suffix=none \
--lineend=linux \
$*
#--ignore-exclude-errors-x \
#--exclude=EASTL \
#--align-reference=name \
apps/systemlib/math/Dcm.cpp
+60 -60
View File
@@ -48,64 +48,64 @@ namespace math
{ {
Dcm::Dcm() : Dcm::Dcm() :
Matrix(Matrix::identity(3)) Matrix(Matrix::identity(3))
{ {
} }
Dcm::Dcm(const float * data) : Dcm::Dcm(const float *data) :
Matrix(3,3,data) Matrix(3, 3, data)
{ {
} }
Dcm::Dcm(const Quaternion & q) : Dcm::Dcm(const Quaternion &q) :
Matrix(3,3) Matrix(3, 3)
{ {
Dcm & dcm = *this; Dcm &dcm = *this;
float a = q.getA(); float a = q.getA();
float b = q.getB(); float b = q.getB();
float c = q.getC(); float c = q.getC();
float d = q.getD(); float d = q.getD();
float aSq = a*a; float aSq = a * a;
float bSq = b*b; float bSq = b * b;
float cSq = c*c; float cSq = c * c;
float dSq = d*d; float dSq = d * d;
dcm(0,0) = aSq + bSq - cSq - dSq; dcm(0, 0) = aSq + bSq - cSq - dSq;
dcm(0,1) = 2*(b*c - a*d); dcm(0, 1) = 2 * (b * c - a * d);
dcm(0,2) = 2*(a*c + b*d); dcm(0, 2) = 2 * (a * c + b * d);
dcm(1,0) = 2*(b*c + a*d); dcm(1, 0) = 2 * (b * c + a * d);
dcm(1,1) = aSq - bSq + cSq - dSq; dcm(1, 1) = aSq - bSq + cSq - dSq;
dcm(1,2) = 2*(c*d - a*b); dcm(1, 2) = 2 * (c * d - a * b);
dcm(2,0) = 2*(b*d - a*c); dcm(2, 0) = 2 * (b * d - a * c);
dcm(2,1) = 2*(a*b + c*d); dcm(2, 1) = 2 * (a * b + c * d);
dcm(2,2) = aSq - bSq - cSq + dSq; dcm(2, 2) = aSq - bSq - cSq + dSq;
} }
Dcm::Dcm(const EulerAngles & euler) : Dcm::Dcm(const EulerAngles &euler) :
Matrix(3,3) Matrix(3, 3)
{ {
Dcm & dcm = *this; Dcm &dcm = *this;
float cosPhi = cosf(euler.getPhi()); float cosPhi = cosf(euler.getPhi());
float sinPhi = sinf(euler.getPhi()); float sinPhi = sinf(euler.getPhi());
float cosThe = cosf(euler.getTheta()); float cosThe = cosf(euler.getTheta());
float sinThe = sinf(euler.getTheta()); float sinThe = sinf(euler.getTheta());
float cosPsi = cosf(euler.getPsi()); float cosPsi = cosf(euler.getPsi());
float sinPsi = sinf(euler.getPsi()); float sinPsi = sinf(euler.getPsi());
dcm(0,0) = cosThe*cosPsi; dcm(0, 0) = cosThe * cosPsi;
dcm(0,1) = -cosPhi*sinPsi + sinPhi*sinThe*cosPsi; dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
dcm(0,2) = sinPhi*sinPsi + cosPhi*sinThe*cosPsi; dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
dcm(1,0) = cosThe*sinPsi; dcm(1, 0) = cosThe * sinPsi;
dcm(1,1) = cosPhi*cosPsi + sinPhi*sinThe*sinPsi; dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
dcm(1,2) = -sinPhi*cosPsi + cosPhi*sinThe*sinPsi; dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
dcm(2,0) = -sinThe; dcm(2, 0) = -sinThe;
dcm(2,1) = sinPhi*cosThe; dcm(2, 1) = sinPhi * cosThe;
dcm(2,2) = cosPhi*cosThe; dcm(2, 2) = cosPhi * cosThe;
} }
Dcm::Dcm(const Dcm & right) : Dcm::Dcm(const Dcm &right) :
Matrix(right) Matrix(right)
{ {
} }
@@ -115,22 +115,22 @@ Dcm::~Dcm()
int __EXPORT dcmTest() int __EXPORT dcmTest()
{ {
printf("Test DCM\t\t: "); printf("Test DCM\t\t: ");
Vector3 vB(1,2,3); Vector3 vB(1, 2, 3);
ASSERT(matrixEqual(Dcm(Quaternion(1,0,0,0)), ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
Matrix::identity(3))); Matrix::identity(3)));
ASSERT(matrixEqual(Dcm(EulerAngles(0,0,0)), ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
Matrix::identity(3))); Matrix::identity(3)));
ASSERT(vectorEqual(Vector3(-2,1,3), ASSERT(vectorEqual(Vector3(-2, 1, 3),
Dcm(EulerAngles(0,0,M_PI_2_F))*vB)); Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
ASSERT(vectorEqual(Vector3(3,2,-1), ASSERT(vectorEqual(Vector3(3, 2, -1),
Dcm(EulerAngles(0,M_PI_2_F,0))*vB)); Dcm(EulerAngles(0, M_PI_2_F, 0))*vB));
ASSERT(vectorEqual(Vector3(1,-3,2), ASSERT(vectorEqual(Vector3(1, -3, 2),
Dcm(EulerAngles(M_PI_2_F,0,0))*vB)); Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB));
ASSERT(vectorEqual(Vector3(3,2,-1), ASSERT(vectorEqual(Vector3(3, 2, -1),
Dcm(EulerAngles( Dcm(EulerAngles(
M_PI_2_F,M_PI_2_F,M_PI_2_F))*vB)); M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
} // namespace math } // namespace math
apps/systemlib/math/Dcm.hpp
+26 -25
View File
@@ -42,7 +42,8 @@
#include "Vector.hpp" #include "Vector.hpp"
#include "Matrix.hpp" #include "Matrix.hpp"
namespace math { namespace math
{
class Quaternion; class Quaternion;
class EulerAngles; class EulerAngles;
@@ -58,35 +59,35 @@ class EulerAngles;
class __EXPORT Dcm : public Matrix class __EXPORT Dcm : public Matrix
{ {
public: public:
/** /**
* default ctor * default ctor
*/ */
Dcm(); Dcm();
/** /**
* data ctor * data ctor
*/ */
Dcm(const float * data); Dcm(const float *data);
/** /**
* quaternion ctor * quaternion ctor
*/ */
Dcm(const Quaternion & q); Dcm(const Quaternion &q);
/** /**
* euler angles ctor * euler angles ctor
*/ */
Dcm(const EulerAngles & euler); Dcm(const EulerAngles &euler);
/** /**
* copy ctor (deep) * copy ctor (deep)
*/ */
Dcm(const Dcm & right); Dcm(const Dcm &right);
/** /**
* dtor * dtor
*/ */
virtual ~Dcm(); virtual ~Dcm();
}; };
int __EXPORT dcmTest(); int __EXPORT dcmTest();
apps/systemlib/math/EulerAngles.cpp
+46 -48
View File
@@ -48,48 +48,46 @@ namespace math
{ {
EulerAngles::EulerAngles() : EulerAngles::EulerAngles() :
Vector(3) Vector(3)
{ {
setPhi(0.0f); setPhi(0.0f);
setTheta(0.0f); setTheta(0.0f);
setPsi(0.0f); setPsi(0.0f);
} }
EulerAngles::EulerAngles(float phi, float theta, float psi) : EulerAngles::EulerAngles(float phi, float theta, float psi) :
Vector(3) Vector(3)
{ {
setPhi(phi); setPhi(phi);
setTheta(theta); setTheta(theta);
setPsi(psi); setPsi(psi);
} }
EulerAngles::EulerAngles(const Quaternion & q) : EulerAngles::EulerAngles(const Quaternion &q) :
Vector(3) Vector(3)
{ {
(*this) = EulerAngles(Dcm(q)); (*this) = EulerAngles(Dcm(q));
} }
EulerAngles::EulerAngles(const Dcm & dcm) : EulerAngles::EulerAngles(const Dcm &dcm) :
Vector(3) Vector(3)
{ {
setTheta(asinf(-dcm(2,0))); setTheta(asinf(-dcm(2, 0)));
if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f)
{ if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f); setPhi(0.0f);
setPsi(atan2f(dcm(1,2) - dcm(0,1), setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0,2) + dcm(1,1)) + getPhi()); dcm(0, 2) + dcm(1, 1)) + getPhi());
}
else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
{ setPhi(0.0f);
setPhi(0.0f); setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
setPsi(atan2f(dcm(1,2) - dcm(0,1), dcm(0, 2) + dcm(1, 1)) - getPhi());
dcm(0,2) + dcm(1,1)) - getPhi());
} } else {
else setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
{ setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
setPhi(atan2f(dcm(2,1),dcm(2,2))); }
setPsi(atan2f(dcm(1,0),dcm(0,0)));
}
} }
EulerAngles::~EulerAngles() EulerAngles::~EulerAngles()
@@ -98,27 +96,27 @@ EulerAngles::~EulerAngles()
int __EXPORT eulerAnglesTest() int __EXPORT eulerAnglesTest()
{ {
printf("Test EulerAngles\t: "); printf("Test EulerAngles\t: ");
EulerAngles euler(1,2,3); EulerAngles euler(1, 2, 3);
// test ctor // test ctor
ASSERT(vectorEqual(Vector3(1,2,3),euler)); ASSERT(vectorEqual(Vector3(1, 2, 3), euler));
ASSERT(equal(euler.getPhi(),1)); ASSERT(equal(euler.getPhi(), 1));
ASSERT(equal(euler.getTheta(),2)); ASSERT(equal(euler.getTheta(), 2));
ASSERT(equal(euler.getPsi(),3)); ASSERT(equal(euler.getPsi(), 3));
// test dcm ctor // test dcm ctor
// test assignment // test assignment
euler.setPhi(4); euler.setPhi(4);
ASSERT(equal(euler.getPhi(),4)); ASSERT(equal(euler.getPhi(), 4));
euler.setTheta(5); euler.setTheta(5);
ASSERT(equal(euler.getTheta(),5)); ASSERT(equal(euler.getTheta(), 5));
euler.setPsi(6); euler.setPsi(6);
ASSERT(equal(euler.getPsi(),6)); ASSERT(equal(euler.getPsi(), 6));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
} // namespace math } // namespace math
apps/systemlib/math/EulerAngles.hpp
+15 -14
View File
@@ -41,7 +41,8 @@
#include "Vector.hpp" #include "Vector.hpp"
namespace math { namespace math
{
class Quaternion; class Quaternion;
class Dcm; class Dcm;
@@ -49,21 +50,21 @@ class Dcm;
class __EXPORT EulerAngles : public Vector class __EXPORT EulerAngles : public Vector
{ {
public: public:
EulerAngles(); EulerAngles();
EulerAngles(float phi, float theta, float psi); EulerAngles(float phi, float theta, float psi);
EulerAngles(const Quaternion & q); EulerAngles(const Quaternion &q);
EulerAngles(const Dcm & dcm); EulerAngles(const Dcm &dcm);
virtual ~EulerAngles(); virtual ~EulerAngles();
// alias // alias
void setPhi(float phi) { (*this)(0) = phi; } void setPhi(float phi) { (*this)(0) = phi; }
void setTheta(float theta) { (*this)(1) = theta; } void setTheta(float theta) { (*this)(1) = theta; }
void setPsi(float psi) { (*this)(2) = psi; } void setPsi(float psi) { (*this)(2) = psi; }
// const accessors // const accessors
const float & getPhi() const { return (*this)(0); } const float &getPhi() const { return (*this)(0); }
const float & getTheta() const { return (*this)(1); } const float &getTheta() const { return (*this)(1); }
const float & getPsi() const { return (*this)(2); } const float &getPsi() const { return (*this)(2); }
}; };
apps/systemlib/math/Matrix.cpp
+112 -99
View File
@@ -45,136 +45,149 @@
namespace math namespace math
{ {
static const float data_testA[] = static const float data_testA[] = {
{1,2,3, 1, 2, 3,
4,5,6}; 4, 5, 6
static Matrix testA(2,3,data_testA); };
static Matrix testA(2, 3, data_testA);
static const float data_testB[] = static const float data_testB[] = {
{0,1,3, 0, 1, 3,
7,-1,2}; 7, -1, 2
static Matrix testB(2,3,data_testB); };
static Matrix testB(2, 3, data_testB);
static const float data_testC[] = static const float data_testC[] = {
{0,1, 0, 1,
2,1, 2, 1,
3,2}; 3, 2
static Matrix testC(3,2,data_testC); };
static Matrix testC(3, 2, data_testC);
static const float data_testD[] = static const float data_testD[] = {
{0,1,2, 0, 1, 2,
2,1,4, 2, 1, 4,
5,2,0}; 5, 2, 0
static Matrix testD(3,3,data_testD); };
static Matrix testD(3, 3, data_testD);
static const float data_testE[] = static const float data_testE[] = {
{1,-1,2, 1, -1, 2,
0,2,3, 0, 2, 3,
2,-1,1}; 2, -1, 1
static Matrix testE(3,3,data_testE); };
static Matrix testE(3, 3, data_testE);
static const float data_testF[] = static const float data_testF[] = {
{3.777e006f, 2.915e007f, 0.000e000f, 3.777e006f, 2.915e007f, 0.000e000f,
2.938e007f, 2.267e008f, 0.000e000f, 2.938e007f, 2.267e008f, 0.000e000f,
0.000e000f, 0.000e000f, 6.033e008f}; 0.000e000f, 0.000e000f, 6.033e008f
static Matrix testF(3,3,data_testF); };
static Matrix testF(3, 3, data_testF);
int __EXPORT matrixTest() int __EXPORT matrixTest()
{ {
matrixAddTest(); matrixAddTest();
matrixSubTest(); matrixSubTest();
matrixMultTest(); matrixMultTest();
matrixInvTest(); matrixInvTest();
matrixDivTest(); matrixDivTest();
return 0; return 0;
} }
int matrixAddTest() int matrixAddTest()
{ {
printf("Test Matrix Add\t\t: "); printf("Test Matrix Add\t\t: ");
Matrix r = testA + testB; Matrix r = testA + testB;
float data_test[] = float data_test[] = {
{ 1.0f, 3.0f, 6.0f, 1.0f, 3.0f, 6.0f,
11.0f, 4.0f, 8.0f}; 11.0f, 4.0f, 8.0f
ASSERT(matrixEqual(Matrix(2,3,data_test),r)); };
printf("PASS\n"); ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
return 0; printf("PASS\n");
return 0;
} }
int matrixSubTest() int matrixSubTest()
{ {
printf("Test Matrix Sub\t\t: "); printf("Test Matrix Sub\t\t: ");
Matrix r = testA - testB; Matrix r = testA - testB;
float data_test[] = float data_test[] = {
{ 1.0f, 1.0f, 0.0f, 1.0f, 1.0f, 0.0f,
-3.0f, 6.0f, 4.0f}; -3.0f, 6.0f, 4.0f
ASSERT(matrixEqual(Matrix(2,3,data_test),r)); };
printf("PASS\n"); ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
return 0; printf("PASS\n");
return 0;
} }
int matrixMultTest() int matrixMultTest()
{ {
printf("Test Matrix Mult\t: "); printf("Test Matrix Mult\t: ");
Matrix r = testC * testB; Matrix r = testC * testB;
float data_test[] = float data_test[] = {
{ 7.0f, -1.0f, 2.0f, 7.0f, -1.0f, 2.0f,
7.0f, 1.0f, 8.0f, 7.0f, 1.0f, 8.0f,
14.0f, 1.0f, 13.0f}; 14.0f, 1.0f, 13.0f
ASSERT(matrixEqual(Matrix(3,3,data_test),r)); };
printf("PASS\n"); ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
return 0; printf("PASS\n");
return 0;
} }
int matrixInvTest() int matrixInvTest()
{ {
printf("Test Matrix Inv\t\t: "); printf("Test Matrix Inv\t\t: ");
Matrix origF = testF; Matrix origF = testF;
Matrix r = testF.inverse(); Matrix r = testF.inverse();
float data_test[] = float data_test[] = {
{ -0.0012518f, 0.0001610f, 0.0000000f, -0.0012518f, 0.0001610f, 0.0000000f,
0.0001622f, -0.0000209f, 0.0000000f, 0.0001622f, -0.0000209f, 0.0000000f,
0.0000000f, 0.0000000f, 1.6580e-9f}; 0.0000000f, 0.0000000f, 1.6580e-9f
ASSERT(matrixEqual(Matrix(3,3,data_test),r)); };
// make sure F in unchanged ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
ASSERT(matrixEqual(origF,testF)); // make sure F in unchanged
printf("PASS\n"); ASSERT(matrixEqual(origF, testF));
return 0; printf("PASS\n");
return 0;
} }
int matrixDivTest() int matrixDivTest()
{ {
printf("Test Matrix Div\t\t: "); printf("Test Matrix Div\t\t: ");
Matrix r = testD / testE; Matrix r = testD / testE;
float data_test[] = { float data_test[] = {
0.2222222f, 0.5555556f, -0.1111111f, 0.2222222f, 0.5555556f, -0.1111111f,
0.0f, 1.0f, 1.0, 0.0f, 1.0f, 1.0,
-4.1111111f, 1.2222222f, 4.5555556f}; -4.1111111f, 1.2222222f, 4.5555556f
ASSERT(matrixEqual(Matrix(3,3,data_test),r)); };
printf("PASS\n"); ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
return 0; printf("PASS\n");
return 0;
} }
bool matrixEqual(const Matrix & a, const Matrix & b, float eps) bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
{ {
if (a.getRows() != b.getRows()) { if (a.getRows() != b.getRows()) {
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
return false; return false;
} else if (a.getCols() != b.getCols()) {
printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); } else if (a.getCols() != b.getCols()) {
return false; 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++) bool ret = true;
{
if (!equal(a(i,j),b(i,j),eps)) for (size_t i = 0; i < a.getRows(); i++)
{ for (size_t j = 0; j < a.getCols(); j++) {
printf("element mismatch (%d, %d)\n", i, j); if (!equal(a(i, j), b(i, j), eps)) {
ret = false; printf("element mismatch (%d, %d)\n", i, j);
} ret = false;
} }
return ret; }
return ret;
} }
} // namespace math } // namespace math
apps/systemlib/math/Matrix.hpp
+3 -2
View File
@@ -45,7 +45,8 @@
#include "generic/Matrix.hpp" #include "generic/Matrix.hpp"
#endif #endif
namespace math { namespace math
{
class Matrix; class Matrix;
int matrixTest(); int matrixTest();
int matrixAddTest(); int matrixAddTest();
@@ -54,5 +55,5 @@ int matrixMultTest();
int matrixInvTest(); int matrixInvTest();
int matrixDivTest(); int matrixDivTest();
int matrixArmTest(); int matrixArmTest();
bool matrixEqual(const Matrix & a, const Matrix & b, float eps=1.0e-5f); bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
} // namespace math } // namespace math
apps/systemlib/math/Quaternion.cpp
+95 -94
View File
@@ -48,68 +48,68 @@ namespace math
{ {
Quaternion::Quaternion() : Quaternion::Quaternion() :
Vector(4) Vector(4)
{ {
setA(1.0f); setA(1.0f);
setB(0.0f); setB(0.0f);
setC(0.0f); setC(0.0f);
setD(0.0f); setD(0.0f);
} }
Quaternion::Quaternion(float a, float b, Quaternion::Quaternion(float a, float b,
float c, float d) : float c, float d) :
Vector(4) Vector(4)
{ {
setA(a); setA(a);
setB(b); setB(b);
setC(c); setC(c);
setD(d); setD(d);
} }
Quaternion::Quaternion(const float * data) : Quaternion::Quaternion(const float *data) :
Vector(4,data) Vector(4, data)
{ {
} }
Quaternion::Quaternion(const Vector & v) : Quaternion::Quaternion(const Vector &v) :
Vector(v) Vector(v)
{ {
} }
Quaternion::Quaternion(const Dcm & dcm) : Quaternion::Quaternion(const Dcm &dcm) :
Vector(4) Vector(4)
{ {
setA(0.5f*sqrtf(1 + dcm(0,0) + setA(0.5f * sqrtf(1 + dcm(0, 0) +
dcm(1,1) + dcm(2,2))); dcm(1, 1) + dcm(2, 2)));
setB((dcm(2,1) - dcm(1,2))/ setB((dcm(2, 1) - dcm(1, 2)) /
(4*getA())); (4 * getA()));
setC((dcm(0,2) - dcm(2,0))/ setC((dcm(0, 2) - dcm(2, 0)) /
(4*getA())); (4 * getA()));
setD((dcm(1,0) - dcm(0,1))/ setD((dcm(1, 0) - dcm(0, 1)) /
(4*getA())); (4 * getA()));
} }
Quaternion::Quaternion(const EulerAngles & euler) : Quaternion::Quaternion(const EulerAngles &euler) :
Vector(4) Vector(4)
{ {
float cosPhi_2 = cosf(euler.getPhi()/2.0f); float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
float cosTheta_2 = cosf(euler.getTheta()/2.0f); float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
float cosPsi_2 = cosf(euler.getPsi()/2.0f); float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
float sinPhi_2 = sinf(euler.getPhi()/2.0f); float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
float sinTheta_2 = sinf(euler.getTheta()/2.0f); float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
float sinPsi_2 = sinf(euler.getPsi()/2.0f); float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
setA(cosPhi_2*cosTheta_2*cosPsi_2 + setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2*sinTheta_2*sinPsi_2); sinPhi_2 * sinTheta_2 * sinPsi_2);
setB(sinPhi_2*cosTheta_2*cosPsi_2 - setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2*sinTheta_2*sinPsi_2); cosPhi_2 * sinTheta_2 * sinPsi_2);
setC(cosPhi_2*sinTheta_2*cosPsi_2 + setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2*cosTheta_2*sinPsi_2); sinPhi_2 * cosTheta_2 * sinPsi_2);
setD(cosPhi_2*cosTheta_2*sinPsi_2 + setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
sinPhi_2*sinTheta_2*cosPsi_2); sinPhi_2 * sinTheta_2 * cosPsi_2);
} }
Quaternion::Quaternion(const Quaternion & right) : Quaternion::Quaternion(const Quaternion &right) :
Vector(right) Vector(right)
{ {
} }
@@ -117,64 +117,65 @@ Quaternion::~Quaternion()
{ {
} }
Vector Quaternion::derivative(const Vector & w) Vector Quaternion::derivative(const Vector &w)
{ {
#ifdef QUATERNION_ASSERT #ifdef QUATERNION_ASSERT
ASSERT(w.getRows()==3); ASSERT(w.getRows() == 3);
#endif #endif
float dataQ[] = float dataQ[] = {
{getA(), -getB(), -getC(), -getD(), getA(), -getB(), -getC(), -getD(),
getB(), getA(), -getD(), getC(), getB(), getA(), -getD(), getC(),
getC(), getD(), getA(), -getB(), getC(), getD(), getA(), -getB(),
getD(), -getC(), getB(), getA()}; getD(), -getC(), getB(), getA()
Vector v(4); };
v(0) = 0.0f; Vector v(4);
v(1) = w(0); v(0) = 0.0f;
v(2) = w(1); v(1) = w(0);
v(3) = w(2); v(2) = w(1);
Matrix Q(4,4,dataQ); v(3) = w(2);
return Q*v*0.5f; Matrix Q(4, 4, dataQ);
return Q * v * 0.5f;
} }
int __EXPORT quaternionTest() int __EXPORT quaternionTest()
{ {
printf("Test Quaternion\t\t: "); printf("Test Quaternion\t\t: ");
// test default ctor // test default ctor
Quaternion q; Quaternion q;
ASSERT(equal(q.getA(),1)); ASSERT(equal(q.getA(), 1));
ASSERT(equal(q.getB(),0)); ASSERT(equal(q.getB(), 0));
ASSERT(equal(q.getC(),0)); ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(),0)); ASSERT(equal(q.getD(), 0));
// test float ctor // test float ctor
q = Quaternion(0,1,0,0); q = Quaternion(0, 1, 0, 0);
ASSERT(equal(q.getA(),0)); ASSERT(equal(q.getA(), 0));
ASSERT(equal(q.getB(),1)); ASSERT(equal(q.getB(), 1));
ASSERT(equal(q.getC(),0)); ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(),0)); ASSERT(equal(q.getD(), 0));
// test euler ctor // test euler ctor
q = Quaternion(EulerAngles(0,0,0)); q = Quaternion(EulerAngles(0, 0, 0));
ASSERT(equal(q.getA(),1)); ASSERT(equal(q.getA(), 1));
ASSERT(equal(q.getB(),0)); ASSERT(equal(q.getB(), 0));
ASSERT(equal(q.getC(),0)); ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(),0)); ASSERT(equal(q.getD(), 0));
// test dcm ctor // test dcm ctor
q = Quaternion(Dcm()); q = Quaternion(Dcm());
ASSERT(equal(q.getA(),1)); ASSERT(equal(q.getA(), 1));
ASSERT(equal(q.getB(),0)); ASSERT(equal(q.getB(), 0));
ASSERT(equal(q.getC(),0)); ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(),0)); ASSERT(equal(q.getD(), 0));
// TODO test derivative // TODO test derivative
// test accessors // test accessors
q.setA(0.1); q.setA(0.1);
q.setB(0.2); q.setB(0.2);
q.setC(0.3); q.setC(0.3);
q.setD(0.4); q.setD(0.4);
ASSERT(equal(q.getA(),0.1)); ASSERT(equal(q.getA(), 0.1));
ASSERT(equal(q.getB(),0.2)); ASSERT(equal(q.getB(), 0.2));
ASSERT(equal(q.getC(),0.3)); ASSERT(equal(q.getC(), 0.3));
ASSERT(equal(q.getD(),0.4)); ASSERT(equal(q.getD(), 0.4));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
} // namespace math } // namespace math
apps/systemlib/math/Quaternion.hpp
+49 -48
View File
@@ -42,7 +42,8 @@
#include "Vector.hpp" #include "Vector.hpp"
#include "Matrix.hpp" #include "Matrix.hpp"
namespace math { namespace math
{
class Dcm; class Dcm;
class EulerAngles; class EulerAngles;
@@ -51,62 +52,62 @@ class __EXPORT Quaternion : public Vector
{ {
public: public:
/** /**
* default ctor * default ctor
*/ */
Quaternion(); Quaternion();
/** /**
* ctor from floats * ctor from floats
*/ */
Quaternion(float a, float b, float c, float d); Quaternion(float a, float b, float c, float d);
/** /**
* ctor from data * ctor from data
*/ */
Quaternion(const float * data); Quaternion(const float *data);
/** /**
* ctor from Vector * ctor from Vector
*/ */
Quaternion(const Vector & v); Quaternion(const Vector &v);
/** /**
* ctor from EulerAngles * ctor from EulerAngles
*/ */
Quaternion(const EulerAngles & euler); Quaternion(const EulerAngles &euler);
/** /**
* ctor from Dcm * ctor from Dcm
*/ */
Quaternion(const Dcm & dcm); Quaternion(const Dcm &dcm);
/** /**
* deep copy ctor * deep copy ctor
*/ */
Quaternion(const Quaternion & right); Quaternion(const Quaternion &right);
/** /**
* dtor * dtor
*/ */
virtual ~Quaternion(); virtual ~Quaternion();
/** /**
* derivative * derivative
*/ */
Vector derivative(const Vector & w); Vector derivative(const Vector &w);
/** /**
* accessors * accessors
*/ */
void setA(float a) { (*this)(0) = a; } void setA(float a) { (*this)(0) = a; }
void setB(float b) { (*this)(1) = b; } void setB(float b) { (*this)(1) = b; }
void setC(float c) { (*this)(2) = c; } void setC(float c) { (*this)(2) = c; }
void setD(float d) { (*this)(3) = d; } void setD(float d) { (*this)(3) = d; }
const float & getA() const { return (*this)(0); } const float &getA() const { return (*this)(0); }
const float & getB() const { return (*this)(1); } const float &getB() const { return (*this)(1); }
const float & getC() const { return (*this)(2); } const float &getC() const { return (*this)(2); }
const float & getD() const { return (*this)(3); } const float &getD() const { return (*this)(3); }
}; };
int __EXPORT quaternionTest(); int __EXPORT quaternionTest();
apps/systemlib/math/Vector.cpp
+36 -35
View File
@@ -44,56 +44,57 @@
namespace math namespace math
{ {
static const float data_testA[] = {1,3}; static const float data_testA[] = {1, 3};
static const float data_testB[] = {4,1}; static const float data_testB[] = {4, 1};
static Vector testA(2,data_testA); static Vector testA(2, data_testA);
static Vector testB(2,data_testB); static Vector testB(2, data_testB);
int __EXPORT vectorTest() int __EXPORT vectorTest()
{ {
vectorAddTest(); vectorAddTest();
vectorSubTest(); vectorSubTest();
return 0; return 0;
} }
int vectorAddTest() int vectorAddTest()
{ {
printf("Test Vector Add\t\t: "); printf("Test Vector Add\t\t: ");
Vector r = testA + testB; Vector r = testA + testB;
float data_test[] = {5.0f, 4.0f}; float data_test[] = {5.0f, 4.0f};
ASSERT(vectorEqual(Vector(2,data_test),r)); ASSERT(vectorEqual(Vector(2, data_test), r));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
int vectorSubTest() int vectorSubTest()
{ {
printf("Test Vector Sub\t\t: "); printf("Test Vector Sub\t\t: ");
Vector r(2); Vector r(2);
r = testA - testB; r = testA - testB;
float data_test[] = {-3.0f, 2.0f}; float data_test[] = { -3.0f, 2.0f};
ASSERT(vectorEqual(Vector(2,data_test),r)); ASSERT(vectorEqual(Vector(2, data_test), r));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
bool vectorEqual(const Vector & a, const Vector & b, float eps) bool vectorEqual(const Vector &a, const Vector &b, float eps)
{ {
if (a.getRows() != b.getRows()) { if (a.getRows() != b.getRows()) {
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
return false; return false;
} }
bool ret = true;
for (size_t i=0;i<a.getRows();i++) bool ret = true;
{
if (!equal(a(i),b(i),eps)) for (size_t i = 0; i < a.getRows(); i++) {
{ if (!equal(a(i), b(i), eps)) {
printf("element mismatch (%d)\n", i); printf("element mismatch (%d)\n", i);
ret = false; ret = false;
} }
} }
return ret;
return ret;
} }
} // namespace math } // namespace math
apps/systemlib/math/Vector.hpp
+3 -2
View File
@@ -45,10 +45,11 @@
#include "generic/Vector.hpp" #include "generic/Vector.hpp"
#endif #endif
namespace math { namespace math
{
class Vector; class Vector;
int __EXPORT vectorTest(); int __EXPORT vectorTest();
int __EXPORT vectorAddTest(); int __EXPORT vectorAddTest();
int __EXPORT vectorSubTest(); int __EXPORT vectorSubTest();
bool vectorEqual(const Vector & a, const Vector & b, float eps=1.0e-5f); bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
} // math } // math
apps/systemlib/math/Vector3.cpp
+25 -25
View File
@@ -45,28 +45,28 @@ namespace math
{ {
Vector3::Vector3() : Vector3::Vector3() :
Vector(3) Vector(3)
{ {
} }
Vector3::Vector3(const Vector & right) : Vector3::Vector3(const Vector &right) :
Vector(right) Vector(right)
{ {
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(right.getRows()==3); ASSERT(right.getRows() == 3);
#endif #endif
} }
Vector3::Vector3(float x, float y, float z) : Vector3::Vector3(float x, float y, float z) :
Vector(3) Vector(3)
{ {
setX(x); setX(x);
setY(y); setY(y);
setZ(z); setZ(z);
} }
Vector3::Vector3(const float * data) : Vector3::Vector3(const float *data) :
Vector(3,data) Vector(3, data)
{ {
} }
@@ -74,26 +74,26 @@ Vector3::~Vector3()
{ {
} }
Vector3 Vector3::cross(const Vector3 & b) Vector3 Vector3::cross(const Vector3 &b)
{ {
Vector3 & a = *this; Vector3 &a = *this;
Vector3 result; Vector3 result;
result(0) = a(1)*b(2) - a(2)*b(1); result(0) = a(1) * b(2) - a(2) * b(1);
result(1) = a(2)*b(0) - a(0)*b(2); result(1) = a(2) * b(0) - a(0) * b(2);
result(2) = a(0)*b(1) - a(1)*b(0); result(2) = a(0) * b(1) - a(1) * b(0);
return result; return result;
} }
int __EXPORT vector3Test() int __EXPORT vector3Test()
{ {
printf("Test Vector3\t\t: "); printf("Test Vector3\t\t: ");
// test float ctor // test float ctor
Vector3 v(1,2,3); Vector3 v(1, 2, 3);
ASSERT(equal(v(0),1)); ASSERT(equal(v(0), 1));
ASSERT(equal(v(1),2)); ASSERT(equal(v(1), 2));
ASSERT(equal(v(2),3)); ASSERT(equal(v(2), 3));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
} }
} // namespace math } // namespace math
apps/systemlib/math/Vector3.hpp
+16 -16
View File
@@ -45,25 +45,25 @@ namespace math
{ {
class __EXPORT Vector3 : class __EXPORT Vector3 :
public Vector public Vector
{ {
public: public:
Vector3(); Vector3();
Vector3(const Vector & right); Vector3(const Vector &right);
Vector3(float x, float y, float z); Vector3(float x, float y, float z);
Vector3(const float * data); Vector3(const float *data);
virtual ~Vector3(); virtual ~Vector3();
Vector3 cross(const Vector3 & b); Vector3 cross(const Vector3 &b);
/** /**
* accessors * accessors
*/ */
void setX(float x) { (*this)(0) = x; } void setX(float x) { (*this)(0) = x; }
void setY(float y) { (*this)(1) = y; } void setY(float y) { (*this)(1) = y; }
void setZ(float z) { (*this)(2) = z; } void setZ(float z) { (*this)(2) = z; }
const float & getX() const { return (*this)(0); } const float &getX() const { return (*this)(0); }
const float & getY() const { return (*this)(1); } const float &getY() const { return (*this)(1); }
const float & getZ() const { return (*this)(2); } const float &getZ() const { return (*this)(2); }
}; };
int __EXPORT vector3Test(); int __EXPORT vector3Test();
apps/systemlib/math/arm/Matrix.hpp
+210 -230
View File
@@ -56,257 +56,237 @@
namespace math namespace math
{ {
class __EXPORT Matrix { class __EXPORT Matrix
{
public: public:
// constructor // constructor
Matrix(size_t rows, size_t cols) : Matrix(size_t rows, size_t cols) :
_matrix() _matrix() {
{ arm_mat_init_f32(&_matrix,
arm_mat_init_f32(&_matrix, rows, cols,
rows, cols, (float *)calloc(rows * cols, sizeof(float)));
(float*)calloc(rows*cols,sizeof(float))); }
} Matrix(size_t rows, size_t cols, const float *data) :
Matrix(size_t rows, size_t cols, const float * data) : _matrix() {
_matrix() arm_mat_init_f32(&_matrix,
{ rows, cols,
arm_mat_init_f32(&_matrix, (float *)malloc(rows * cols * sizeof(float)));
rows, cols, memcpy(getData(), data, getSize());
(float*)malloc(rows*cols*sizeof(float))); }
memcpy(getData(),data,getSize()); // deconstructor
} virtual ~Matrix() {
// deconstructor delete [] _matrix.pData;
virtual ~Matrix() }
{ // copy constructor (deep)
delete [] _matrix.pData; Matrix(const Matrix &right) :
} _matrix() {
// copy constructor (deep) arm_mat_init_f32(&_matrix,
Matrix(const Matrix & right) : right.getRows(), right.getCols(),
_matrix() (float *)malloc(right.getRows()*
{ right.getCols()*sizeof(float)));
arm_mat_init_f32(&_matrix, memcpy(getData(), right.getData(),
right.getRows(), right.getCols(), getSize());
(float*)malloc(right.getRows()* }
right.getCols()*sizeof(float))); // assignment
memcpy(getData(),right.getData(), inline Matrix &operator=(const Matrix &right) {
getSize());
}
// assignment
inline Matrix & operator=(const Matrix & right)
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols()==right.getCols()); ASSERT(getCols() == right.getCols());
#endif #endif
if (this != &right)
{ if (this != &right) {
memcpy(getData(),right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
return *this;
} return *this;
// element accessors }
inline float & operator()(size_t i, size_t j) // element accessors
{ inline float &operator()(size_t i, size_t j) {
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(i<getRows()); ASSERT(i < getRows());
ASSERT(j<getCols()); ASSERT(j < getCols());
#endif #endif
return getData()[i*getCols() + j]; return getData()[i * getCols() + j];
} }
inline const float & operator()(size_t i, size_t j) const inline const float &operator()(size_t i, size_t j) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(i<getRows()); ASSERT(i < getRows());
ASSERT(j<getCols()); ASSERT(j < getCols());
#endif #endif
return getData()[i*getCols() + j]; return getData()[i * getCols() + j];
} }
// output // output
inline void print() const inline void print() const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++) for (size_t j = 0; j < getCols(); j++) {
{ float sig;
for (size_t j=0; j<getCols(); j++) int exp;
{ float num = (*this)(i, j);
float sig; float2SigExp(num, sig, exp);
int exp; printf("%6.3fe%03.3d,", (double)sig, exp);
float num = (*this)(i,j); }
float2SigExp(num,sig,exp);
printf ("%6.3fe%03.3d,", (double)sig, exp); printf("\n");
} }
printf("\n"); }
} // boolean ops
} inline bool operator==(const Matrix &right) const {
// boolean ops for (size_t i = 0; i < getRows(); i++) {
inline bool operator==(const Matrix & right) const for (size_t j = 0; j < getCols(); j++) {
{ if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
for (size_t i=0; i<getRows(); i++) return false;
{ }
for (size_t j=0; j<getCols(); j++) }
{
if (fabsf((*this)(i,j)-right(i,j)) > 1e-30f) return true;
return false; }
} // scalar ops
} inline Matrix operator+(float right) const {
return true; Matrix result(getRows(), getCols());
} arm_offset_f32((float *)getData(), right,
// scalar ops (float *)result.getData(), getRows()*getCols());
inline Matrix operator+(float right) const return result;
{ }
Matrix result(getRows(), getCols()); inline Matrix operator-(float right) const {
arm_offset_f32((float *)getData(),right, Matrix result(getRows(), getCols());
(float *)result.getData(),getRows()*getCols()); arm_offset_f32((float *)getData(), -right,
return result; (float *)result.getData(), getRows()*getCols());
} return result;
inline Matrix operator-(float right) const }
{ inline Matrix operator*(float right) const {
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
arm_offset_f32((float *)getData(),-right, arm_mat_scale_f32(&_matrix, right,
(float *)result.getData(),getRows()*getCols()); &(result._matrix));
return result; return result;
} }
inline Matrix operator*(float right) const inline Matrix operator/(float right) const {
{ Matrix result(getRows(), getCols());
Matrix result(getRows(), getCols()); arm_mat_scale_f32(&_matrix, 1.0f / right,
arm_mat_scale_f32(&_matrix,right, &(result._matrix));
&(result._matrix)); return result;
return result; }
} // vector ops
inline Matrix operator/(float right) const inline Vector operator*(const Vector &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 #ifdef MATRIX_ASSERT
ASSERT(getCols()==right.getRows()); ASSERT(getCols() == right.getRows());
#endif #endif
Matrix resultMat = (*this)* Matrix resultMat = (*this) *
Matrix(right.getRows(),1,right.getData()); Matrix(right.getRows(), 1, right.getData());
return Vector(getRows(),resultMat.getData()); return Vector(getRows(), resultMat.getData());
} }
// matrix ops // matrix ops
inline Matrix operator+(const Matrix & right) const inline Matrix operator+(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols()==right.getCols()); ASSERT(getCols() == right.getCols());
#endif #endif
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
arm_mat_add_f32(&_matrix, &(right._matrix), arm_mat_add_f32(&_matrix, &(right._matrix),
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline Matrix operator-(const Matrix & right) const inline Matrix operator-(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols()==right.getCols()); ASSERT(getCols() == right.getCols());
#endif #endif
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
arm_mat_sub_f32(&_matrix, &(right._matrix), arm_mat_sub_f32(&_matrix, &(right._matrix),
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline Matrix operator*(const Matrix & right) const inline Matrix operator*(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getCols()==right.getRows()); ASSERT(getCols() == right.getRows());
#endif #endif
Matrix result(getRows(), right.getCols()); Matrix result(getRows(), right.getCols());
arm_mat_mult_f32(&_matrix, &(right._matrix), arm_mat_mult_f32(&_matrix, &(right._matrix),
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline Matrix operator/(const Matrix & right) const inline Matrix operator/(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(right.getRows()==right.getCols()); ASSERT(right.getRows() == right.getCols());
ASSERT(getCols()==right.getCols()); ASSERT(getCols() == right.getCols());
#endif #endif
return (*this)*right.inverse(); return (*this) * right.inverse();
} }
// other functions // other functions
inline Matrix transpose() const inline Matrix transpose() const {
{ Matrix result(getCols(), getRows());
Matrix result(getCols(),getRows()); arm_mat_trans_f32(&_matrix, &(result._matrix));
arm_mat_trans_f32(&_matrix, &(result._matrix)); return result;
return result; }
} inline void swapRows(size_t a, size_t b) {
inline void swapRows(size_t a, size_t b) if (a == b) return;
{
if (a==b) return; for (size_t j = 0; j < getCols(); j++) {
for(size_t j=0;j<getCols();j++) { float tmp = (*this)(a, j);
float tmp = (*this)(a,j); (*this)(a, j) = (*this)(b, j);
(*this)(a,j) = (*this)(b,j); (*this)(b, j) = tmp;
(*this)(b,j) = tmp; }
} }
} inline void swapCols(size_t a, size_t b) {
inline void swapCols(size_t a, size_t b) if (a == b) return;
{
if (a==b) return; for (size_t i = 0; i < getRows(); i++) {
for(size_t i=0;i<getRows();i++) { float tmp = (*this)(i, a);
float tmp = (*this)(i,a); (*this)(i, a) = (*this)(i, b);
(*this)(i,a) = (*this)(i,b); (*this)(i, b) = tmp;
(*this)(i,b) = tmp; }
} }
} /**
/** * inverse based on LU factorization with partial pivotting
* inverse based on LU factorization with partial pivotting */
*/ Matrix inverse() const {
Matrix inverse() const
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows()==getCols()); ASSERT(getRows() == getCols());
#endif #endif
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
Matrix work = (*this); Matrix work = (*this);
arm_mat_inverse_f32(&(work._matrix), arm_mat_inverse_f32(&(work._matrix),
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline void setAll(const float & val) inline void setAll(const float &val) {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0;i<getRows();i++) { for (size_t j = 0; j < getCols(); j++) {
for (size_t j=0;j<getCols();j++) { (*this)(i, j) = val;
(*this)(i,j) = val; }
} }
} }
} inline void set(const float *data) {
inline void set(const float * data) memcpy(getData(), data, getSize());
{ }
memcpy(getData(),data,getSize()); inline size_t getRows() const { return _matrix.numRows; }
} inline size_t getCols() const { return _matrix.numCols; }
inline size_t getRows() const { return _matrix.numRows; } inline static Matrix identity(size_t size) {
inline size_t getCols() const { return _matrix.numCols; } Matrix result(size, size);
inline static Matrix identity(size_t size) {
Matrix result(size,size); for (size_t i = 0; i < size; i++) {
for (size_t i=0; i<size; i++) { result(i, i) = 1.0f;
result(i,i) = 1.0f; }
}
return result; return result;
} }
inline static Matrix zero(size_t size) { inline static Matrix zero(size_t size) {
Matrix result(size,size); Matrix result(size, size);
result.setAll(0.0f); result.setAll(0.0f);
return result; return result;
} }
inline static Matrix zero(size_t m, size_t n) { inline static Matrix zero(size_t m, size_t n) {
Matrix result(m,n); Matrix result(m, n);
result.setAll(0.0f); result.setAll(0.0f);
return result; return result;
} }
protected: protected:
inline size_t getSize() const { return sizeof(float)*getRows()*getCols(); } inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
inline float * getData() { return _matrix.pData; } inline float *getData() { return _matrix.pData; }
inline const float * getData() const { return _matrix.pData; } inline const float *getData() const { return _matrix.pData; }
inline void setData(float * data) { _matrix.pData = data; } inline void setData(float *data) { _matrix.pData = data; }
private: private:
arm_matrix_instance_f32 _matrix; arm_matrix_instance_f32 _matrix;
}; };
} // namespace math } // namespace math
apps/systemlib/math/arm/Vector.hpp
+147 -167
View File
@@ -55,186 +55,166 @@
namespace math namespace math
{ {
class __EXPORT Vector { class __EXPORT Vector
{
public: public:
// constructor // constructor
Vector(size_t rows) : Vector(size_t rows) :
_rows(rows), _rows(rows),
_data((float*)calloc(rows,sizeof(float))) _data((float *)calloc(rows, sizeof(float))) {
{ }
} Vector(size_t rows, const float *data) :
Vector(size_t rows, const float * data) : _rows(rows),
_rows(rows), _data((float *)malloc(getSize())) {
_data((float*)malloc(getSize())) memcpy(getData(), data, getSize());
{ }
memcpy(getData(),data,getSize()); // deconstructor
} virtual ~Vector() {
// deconstructor delete [] getData();
virtual ~Vector() }
{ // copy constructor (deep)
delete [] getData(); Vector(const Vector &right) :
} _rows(right.getRows()),
// copy constructor (deep) _data((float *)malloc(getSize())) {
Vector(const Vector & right) : memcpy(getData(), right.getData(),
_rows(right.getRows()), right.getSize());
_data((float*)malloc(getSize())) }
{ // assignment
memcpy(getData(),right.getData(), inline Vector &operator=(const Vector &right) {
right.getSize());
}
// assignment
inline Vector & operator=(const Vector & right)
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
if (this != &right)
{ if (this != &right) {
memcpy(getData(),right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
return *this;
} return *this;
// element accessors }
inline float& operator()(size_t i) // element accessors
{ inline float &operator()(size_t i) {
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(i<getRows()); ASSERT(i < getRows());
#endif #endif
return getData()[i]; return getData()[i];
} }
inline const float& operator()(size_t i) const inline const float &operator()(size_t i) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(i<getRows()); ASSERT(i < getRows());
#endif #endif
return getData()[i]; return getData()[i];
} }
// output // output
inline void print() const inline void print() const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++) float sig;
{ int exp;
float sig; float num = (*this)(i);
int exp; float2SigExp(num, sig, exp);
float num = (*this)(i); printf("%6.3fe%03.3d,", (double)sig, exp);
float2SigExp(num,sig,exp); }
printf ("%6.3fe%03.3d,", (double)sig, exp);
} printf("\n");
printf("\n"); }
} // boolean ops
// boolean ops inline bool operator==(const Vector &right) const {
inline bool operator==(const Vector & right) const for (size_t i = 0; i < getRows(); i++) {
{ if (fabsf(((*this)(i) - right(i))) > 1e-30f)
for (size_t i=0; i<getRows(); i++) return false;
{ }
if (fabsf(((*this)(i) - right(i))) > 1e-30f)
return false; return true;
} }
return true; // scalar ops
} inline Vector operator+(float right) const {
// scalar ops Vector result(getRows());
inline Vector operator+(float right) const arm_offset_f32((float *)getData(),
{ right, result.getData(),
Vector result(getRows()); getRows());
arm_offset_f32((float*)getData(), return result;
right, result.getData(), }
getRows()); inline Vector operator-(float right) const {
return result; Vector result(getRows());
} arm_offset_f32((float *)getData(),
inline Vector operator-(float right) const -right, result.getData(),
{ getRows());
Vector result(getRows()); return result;
arm_offset_f32((float*)getData(), }
-right, result.getData(), inline Vector operator*(float right) const {
getRows()); Vector result(getRows());
return result; arm_scale_f32((float *)getData(),
} right, result.getData(),
inline Vector operator*(float right) const getRows());
{ return result;
Vector result(getRows()); }
arm_scale_f32((float*)getData(), inline Vector operator/(float right) const {
right, result.getData(), Vector result(getRows());
getRows()); arm_scale_f32((float *)getData(),
return result; 1.0f / right, result.getData(),
} getRows());
inline Vector operator/(float right) const return result;
{ }
Vector result(getRows()); // vector ops
arm_scale_f32((float*)getData(), inline Vector operator+(const Vector &right) const {
1.0f/right, result.getData(),
getRows());
return result;
}
// vector ops
inline Vector operator+(const Vector & right) const
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
Vector result(getRows()); Vector result(getRows());
arm_add_f32((float*)getData(), arm_add_f32((float *)getData(),
(float*)right.getData(), (float *)right.getData(),
result.getData(), result.getData(),
getRows()); getRows());
return result; return result;
} }
inline Vector operator-(const Vector & right) const inline Vector operator-(const Vector &right) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
Vector result(getRows()); Vector result(getRows());
arm_sub_f32((float*)getData(), arm_sub_f32((float *)getData(),
(float*)right.getData(), (float *)right.getData(),
result.getData(), result.getData(),
getRows()); getRows());
return result; return result;
} }
// other functions // other functions
inline float dot(const Vector & right) inline float dot(const Vector &right) {
{ float result = 0;
float result = 0; arm_dot_prod_f32((float *)getData(),
arm_dot_prod_f32((float*)getData(), (float *)right.getData(),
(float*)right.getData(), getRows(),
getRows(), &result);
&result); return result;
return result; }
} inline float norm() {
inline float norm() return sqrtf(dot(*this));
{ }
return sqrtf(dot(*this)); inline Vector unit() {
} return (*this) / norm();
inline Vector unit() }
{ inline static Vector zero(size_t rows) {
return (*this)/norm(); Vector result(rows);
} // calloc returns zeroed memory
inline static Vector zero(size_t rows) return result;
{ }
Vector result(rows); inline void setAll(const float &val) {
// calloc returns zeroed memory for (size_t i = 0; i < getRows(); i++) {
return result; (*this)(i) = val;
} }
inline void setAll(const float & val) }
{ inline void set(const float *data) {
for (size_t i=0;i<getRows();i++) memcpy(getData(), data, getSize());
{ }
(*this)(i) = val; inline size_t getRows() const { return _rows; }
} inline const float *getData() const { return _data; }
}
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: protected:
inline size_t getSize() const { return sizeof(float)*getRows(); } inline size_t getSize() const { return sizeof(float) * getRows(); }
inline float * getData() { return _data; } inline float *getData() { return _data; }
inline void setData(float * data) { _data = data; } inline void setData(float *data) { _data = data; }
private: private:
size_t _rows; size_t _rows;
float * _data; float *_data;
}; };
} // math } // math
apps/systemlib/math/generic/Matrix.hpp
+365 -375
View File
File diff suppressed because it is too large Load Diff
apps/systemlib/math/generic/Vector.hpp
+158 -171
View File
@@ -51,190 +51,177 @@
namespace math namespace math
{ {
class __EXPORT Vector { class __EXPORT Vector
{
public: public:
// constructor // constructor
Vector(size_t rows) : Vector(size_t rows) :
_rows(rows), _rows(rows),
_data((float*)calloc(rows,sizeof(float))) _data((float *)calloc(rows, sizeof(float))) {
{ }
} Vector(size_t rows, const float *data) :
Vector(size_t rows, const float * data) : _rows(rows),
_rows(rows), _data((float *)malloc(getSize())) {
_data((float*)malloc(getSize())) memcpy(getData(), data, getSize());
{ }
memcpy(getData(),data,getSize()); // deconstructor
} virtual ~Vector() {
// deconstructor delete [] getData();
virtual ~Vector() }
{ // copy constructor (deep)
delete [] getData(); Vector(const Vector &right) :
} _rows(right.getRows()),
// copy constructor (deep) _data((float *)malloc(getSize())) {
Vector(const Vector & right) : memcpy(getData(), right.getData(),
_rows(right.getRows()), right.getSize());
_data((float*)malloc(getSize())) }
{ // assignment
memcpy(getData(),right.getData(), inline Vector &operator=(const Vector &right) {
right.getSize());
}
// assignment
inline Vector & operator=(const Vector & right)
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
if (this != &right)
{ if (this != &right) {
memcpy(getData(),right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
return *this;
} return *this;
// element accessors }
inline float& operator()(size_t i) // element accessors
{ inline float &operator()(size_t i) {
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(i<getRows()); ASSERT(i < getRows());
#endif #endif
return getData()[i]; return getData()[i];
} }
inline const float& operator()(size_t i) const inline const float &operator()(size_t i) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(i<getRows()); ASSERT(i < getRows());
#endif #endif
return getData()[i]; return getData()[i];
} }
// output // output
inline void print() const inline void print() const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++) float sig;
{ int exp;
float sig; float num = (*this)(i);
int exp; float2SigExp(num, sig, exp);
float num = (*this)(i); printf("%6.3fe%03.3d,", (double)sig, exp);
float2SigExp(num,sig,exp); }
printf ("%6.3fe%03.3d,", (double)sig, exp);
} printf("\n");
printf("\n"); }
} // boolean ops
// boolean ops inline bool operator==(const Vector &right) const {
inline bool operator==(const Vector & right) const for (size_t i = 0; i < getRows(); i++) {
{ if (fabsf(((*this)(i) - right(i))) > 1e-30f)
for (size_t i=0; i<getRows(); i++) return false;
{ }
if (fabsf(((*this)(i) - right(i))) > 1e-30f)
return false; return true;
} }
return true; // scalar ops
} inline Vector operator+(const float &right) const {
// scalar ops Vector result(getRows());
inline Vector operator+(const float & right) const
{ for (size_t i = 0; i < getRows(); i++) {
Vector result(getRows()); result(i) = (*this)(i) + right;
for (size_t i=0; i<getRows(); i++) }
{
result(i) = (*this)(i) + right; return result;
} }
return result; inline Vector operator-(const float &right) const {
} Vector result(getRows());
inline Vector operator-(const float & right) const
{ for (size_t i = 0; i < getRows(); i++) {
Vector result(getRows()); result(i) = (*this)(i) - right;
for (size_t i=0; i<getRows(); i++) }
{
result(i) = (*this)(i) - right; return result;
} }
return result; inline Vector operator*(const float &right) const {
} Vector result(getRows());
inline Vector operator*(const float & right) const
{ for (size_t i = 0; i < getRows(); i++) {
Vector result(getRows()); result(i) = (*this)(i) * right;
for (size_t i=0; i<getRows(); i++) }
{
result(i) = (*this)(i) * right; return result;
} }
return result; inline Vector operator/(const float &right) const {
} Vector result(getRows());
inline Vector operator/(const float & right) const
{ for (size_t i = 0; i < getRows(); i++) {
Vector result(getRows()); result(i) = (*this)(i) / right;
for (size_t i=0; i<getRows(); i++) }
{
result(i) = (*this)(i) / right; return result;
} }
return result; // vector ops
} inline Vector operator+(const Vector &right) const {
// vector ops
inline Vector operator+(const Vector & right) const
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result(i) = (*this)(i) + right(i); result(i) = (*this)(i) + right(i);
} }
return result;
} return result;
inline Vector operator-(const Vector & right) const }
{ inline Vector operator-(const Vector &right) const {
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows()==right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result(i) = (*this)(i) - right(i); result(i) = (*this)(i) - right(i);
} }
return result;
} return result;
// other functions }
inline float dot(const Vector & right) // other functions
{ inline float dot(const Vector &right) {
float result = 0; float result = 0;
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result += (*this)(i)*(*this)(i); result += (*this)(i) * (*this)(i);
} }
return result;
} return result;
inline float norm() }
{ inline float norm() {
return sqrtf(dot(*this)); return sqrtf(dot(*this));
} }
inline Vector unit() inline Vector unit() {
{ return (*this) / norm();
return (*this)/norm(); }
} inline static Vector zero(size_t rows) {
inline static Vector zero(size_t rows) Vector result(rows);
{ // calloc returns zeroed memory
Vector result(rows); return result;
// calloc returns zeroed memory }
return result; inline void setAll(const float &val) {
} for (size_t i = 0; i < getRows(); i++) {
inline void setAll(const float & val) (*this)(i) = val;
{ }
for (size_t i=0;i<getRows();i++) }
{ inline void set(const float *data) {
(*this)(i) = val; memcpy(getData(), data, getSize());
} }
} inline size_t getRows() const { return _rows; }
inline void set(const float * data)
{
memcpy(getData(),data,getSize());
}
inline size_t getRows() const { return _rows; }
protected: protected:
inline size_t getSize() const { return sizeof(float)*getRows(); } inline size_t getSize() const { return sizeof(float) * getRows(); }
inline float * getData() { return _data; } inline float *getData() { return _data; }
inline const float * getData() const { return _data; } inline const float *getData() const { return _data; }
inline void setData(float * data) { _data = data; } inline void setData(float *data) { _data = data; }
private: private:
size_t _rows; size_t _rows;
float * _data; float *_data;
}; };
} // math } // math
apps/systemlib/test/test.cpp
+40 -35
View File
@@ -44,45 +44,50 @@
bool __EXPORT equal(float a, float b, float epsilon) bool __EXPORT equal(float a, float b, float epsilon)
{ {
float diff = fabsf(a-b); float diff = fabsf(a - b);
if (diff>epsilon)
{ if (diff > epsilon) {
printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false; return false;
}
else return true; } else return true;
} }
void __EXPORT float2SigExp( void __EXPORT float2SigExp(
const float & num, const float &num,
float & sig, float &sig,
int & exp) int &exp)
{ {
if (isnan(num) || isinf(num)) if (isnan(num) || isinf(num)) {
{ sig = 0.0f;
sig = 0.0f; exp = -99;
exp = -99; return;
return; }
}
if (fabsf(num) < 1.0e-38f) if (fabsf(num) < 1.0e-38f) {
{ sig = 0;
sig = 0; exp = 0;
exp = 0; return;
return; }
}
exp = log10f(fabsf(num)); exp = log10f(fabsf(num));
if (exp>0) {
exp = ceil(exp); if (exp > 0) {
} else { exp = ceil(exp);
exp = floor(exp);
} } else {
sig = num; exp = floor(exp);
// cheap power since it is integer }
if (exp>0) {
for (int i=0;i<abs(exp);i++) sig /= 10; sig = num;
} else {
for (int i=0;i<abs(exp);i++) sig *= 10; // cheap power since it is integer
} if (exp > 0) {
for (int i = 0; i < abs(exp); i++) sig /= 10;
} else {
for (int i = 0; i < abs(exp); i++) sig *= 10;
}
} }
apps/systemlib/test/test.hpp
+3 -3
View File
@@ -45,6 +45,6 @@
bool equal(float a, float b, float eps = 1e-5); bool equal(float a, float b, float eps = 1e-5);
void float2SigExp( void float2SigExp(
const float & num, const float &num,
float & sig, float &sig,
int & exp); int &exp);