diff options
-rwxr-xr-x | Tools/fix_code_style_ubuntu.sh | 19 | ||||
-rw-r--r-- | apps/systemlib/math/Dcm.cpp | 120 | ||||
-rw-r--r-- | apps/systemlib/math/Dcm.hpp | 53 | ||||
-rw-r--r-- | apps/systemlib/math/EulerAngles.cpp | 102 | ||||
-rw-r--r-- | apps/systemlib/math/EulerAngles.hpp | 29 | ||||
-rw-r--r-- | apps/systemlib/math/Matrix.cpp | 223 | ||||
-rw-r--r-- | apps/systemlib/math/Matrix.hpp | 5 | ||||
-rw-r--r-- | apps/systemlib/math/Quaternion.cpp | 189 | ||||
-rw-r--r-- | apps/systemlib/math/Quaternion.hpp | 115 | ||||
-rw-r--r-- | apps/systemlib/math/Vector.cpp | 71 | ||||
-rw-r--r-- | apps/systemlib/math/Vector.hpp | 5 | ||||
-rw-r--r-- | apps/systemlib/math/Vector3.cpp | 50 | ||||
-rw-r--r-- | apps/systemlib/math/Vector3.hpp | 32 | ||||
-rw-r--r-- | apps/systemlib/math/arm/Matrix.hpp | 440 | ||||
-rw-r--r-- | apps/systemlib/math/arm/Vector.hpp | 314 | ||||
-rw-r--r-- | apps/systemlib/math/generic/Matrix.hpp | 722 | ||||
-rw-r--r-- | apps/systemlib/math/generic/Vector.hpp | 329 | ||||
-rw-r--r-- | apps/systemlib/test/test.cpp | 75 | ||||
-rw-r--r-- | apps/systemlib/test/test.hpp | 6 |
19 files changed, 1439 insertions, 1460 deletions
diff --git a/Tools/fix_code_style_ubuntu.sh b/Tools/fix_code_style_ubuntu.sh new file mode 100755 index 000000000..90ab57b89 --- /dev/null +++ b/Tools/fix_code_style_ubuntu.sh @@ -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 \ diff --git a/apps/systemlib/math/Dcm.cpp b/apps/systemlib/math/Dcm.cpp index 1e9c4ee6d..b9f4309cc 100644 --- a/apps/systemlib/math/Dcm.cpp +++ b/apps/systemlib/math/Dcm.cpp @@ -48,64 +48,64 @@ namespace math { Dcm::Dcm() : - Matrix(Matrix::identity(3)) + Matrix(Matrix::identity(3)) { } -Dcm::Dcm(const float * data) : - Matrix(3,3,data) +Dcm::Dcm(const float *data) : + Matrix(3, 3, data) { } -Dcm::Dcm(const Quaternion & q) : - Matrix(3,3) +Dcm::Dcm(const Quaternion &q) : + Matrix(3, 3) { - Dcm & dcm = *this; - float a = q.getA(); - float b = q.getB(); - float c = q.getC(); - float d = q.getD(); - float aSq = a*a; - float bSq = b*b; - float cSq = c*c; - float dSq = d*d; - dcm(0,0) = aSq + bSq - cSq - dSq; - dcm(0,1) = 2*(b*c - a*d); - dcm(0,2) = 2*(a*c + b*d); - dcm(1,0) = 2*(b*c + a*d); - dcm(1,1) = aSq - bSq + cSq - dSq; - dcm(1,2) = 2*(c*d - a*b); - dcm(2,0) = 2*(b*d - a*c); - dcm(2,1) = 2*(a*b + c*d); - dcm(2,2) = aSq - bSq - cSq + dSq; + Dcm &dcm = *this; + float a = q.getA(); + float b = q.getB(); + float c = q.getC(); + float d = q.getD(); + float aSq = a * a; + float bSq = b * b; + float cSq = c * c; + float dSq = d * d; + dcm(0, 0) = aSq + bSq - cSq - dSq; + dcm(0, 1) = 2 * (b * c - a * d); + dcm(0, 2) = 2 * (a * c + b * d); + dcm(1, 0) = 2 * (b * c + a * d); + dcm(1, 1) = aSq - bSq + cSq - dSq; + dcm(1, 2) = 2 * (c * d - a * b); + dcm(2, 0) = 2 * (b * d - a * c); + dcm(2, 1) = 2 * (a * b + c * d); + dcm(2, 2) = aSq - bSq - cSq + dSq; } -Dcm::Dcm(const EulerAngles & euler) : - Matrix(3,3) +Dcm::Dcm(const EulerAngles &euler) : + Matrix(3, 3) { - Dcm & dcm = *this; - float cosPhi = cosf(euler.getPhi()); - float sinPhi = sinf(euler.getPhi()); - float cosThe = cosf(euler.getTheta()); - float sinThe = sinf(euler.getTheta()); - float cosPsi = cosf(euler.getPsi()); - float sinPsi = sinf(euler.getPsi()); + Dcm &dcm = *this; + float cosPhi = cosf(euler.getPhi()); + float sinPhi = sinf(euler.getPhi()); + float cosThe = cosf(euler.getTheta()); + float sinThe = sinf(euler.getTheta()); + float cosPsi = cosf(euler.getPsi()); + float sinPsi = sinf(euler.getPsi()); - dcm(0,0) = cosThe*cosPsi; - dcm(0,1) = -cosPhi*sinPsi + sinPhi*sinThe*cosPsi; - dcm(0,2) = sinPhi*sinPsi + cosPhi*sinThe*cosPsi; + dcm(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(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(2, 0) = -sinThe; + dcm(2, 1) = sinPhi * cosThe; + dcm(2, 2) = cosPhi * cosThe; } -Dcm::Dcm(const Dcm & right) : - Matrix(right) +Dcm::Dcm(const Dcm &right) : + Matrix(right) { } @@ -115,22 +115,22 @@ Dcm::~Dcm() int __EXPORT dcmTest() { - printf("Test DCM\t\t: "); - Vector3 vB(1,2,3); - ASSERT(matrixEqual(Dcm(Quaternion(1,0,0,0)), - Matrix::identity(3))); - ASSERT(matrixEqual(Dcm(EulerAngles(0,0,0)), - Matrix::identity(3))); - ASSERT(vectorEqual(Vector3(-2,1,3), - Dcm(EulerAngles(0,0,M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3,2,-1), - Dcm(EulerAngles(0,M_PI_2_F,0))*vB)); - ASSERT(vectorEqual(Vector3(1,-3,2), - Dcm(EulerAngles(M_PI_2_F,0,0))*vB)); - ASSERT(vectorEqual(Vector3(3,2,-1), - Dcm(EulerAngles( - M_PI_2_F,M_PI_2_F,M_PI_2_F))*vB)); - printf("PASS\n"); - return 0; + printf("Test DCM\t\t: "); + Vector3 vB(1, 2, 3); + ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)), + Matrix::identity(3))); + ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)), + Matrix::identity(3))); + ASSERT(vectorEqual(Vector3(-2, 1, 3), + Dcm(EulerAngles(0, 0, M_PI_2_F))*vB)); + ASSERT(vectorEqual(Vector3(3, 2, -1), + Dcm(EulerAngles(0, M_PI_2_F, 0))*vB)); + ASSERT(vectorEqual(Vector3(1, -3, 2), + Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB)); + ASSERT(vectorEqual(Vector3(3, 2, -1), + Dcm(EulerAngles( + M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); + printf("PASS\n"); + return 0; } } // namespace math diff --git a/apps/systemlib/math/Dcm.hpp b/apps/systemlib/math/Dcm.hpp index 658d902fa..de69a7aa4 100644 --- a/apps/systemlib/math/Dcm.hpp +++ b/apps/systemlib/math/Dcm.hpp @@ -42,7 +42,8 @@ #include "Vector.hpp" #include "Matrix.hpp" -namespace math { +namespace math +{ class Quaternion; class EulerAngles; @@ -52,41 +53,41 @@ class EulerAngles; * (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 + * as C_nb. C_bn can be obtained through use * of the transpose() method. */ class __EXPORT Dcm : public Matrix { public: - /** - * default ctor - */ - Dcm(); + /** + * default ctor + */ + Dcm(); - /** - * data ctor - */ - Dcm(const float * data); + /** + * data ctor + */ + Dcm(const float *data); - /** - * quaternion ctor - */ - Dcm(const Quaternion & q); + /** + * quaternion ctor + */ + Dcm(const Quaternion &q); - /** - * euler angles ctor - */ - Dcm(const EulerAngles & euler); + /** + * euler angles ctor + */ + Dcm(const EulerAngles &euler); - /** - * copy ctor (deep) - */ - Dcm(const Dcm & right); + /** + * copy ctor (deep) + */ + Dcm(const Dcm &right); - /** - * dtor - */ - virtual ~Dcm(); + /** + * dtor + */ + virtual ~Dcm(); }; int __EXPORT dcmTest(); diff --git a/apps/systemlib/math/EulerAngles.cpp b/apps/systemlib/math/EulerAngles.cpp index b530f172d..259c866c7 100644 --- a/apps/systemlib/math/EulerAngles.cpp +++ b/apps/systemlib/math/EulerAngles.cpp @@ -48,48 +48,46 @@ namespace math { EulerAngles::EulerAngles() : - Vector(3) + Vector(3) { - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); + setPhi(0.0f); + setTheta(0.0f); + setPsi(0.0f); } EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) + Vector(3) { - setPhi(phi); - setTheta(theta); - setPsi(psi); + setPhi(phi); + setTheta(theta); + setPsi(psi); } -EulerAngles::EulerAngles(const Quaternion & q) : - Vector(3) +EulerAngles::EulerAngles(const Quaternion &q) : + Vector(3) { - (*this) = EulerAngles(Dcm(q)); + (*this) = EulerAngles(Dcm(q)); } -EulerAngles::EulerAngles(const Dcm & dcm) : - Vector(3) +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))); - } + 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() @@ -98,27 +96,27 @@ EulerAngles::~EulerAngles() int __EXPORT eulerAnglesTest() { - printf("Test EulerAngles\t: "); - EulerAngles euler(1,2,3); - - // test ctor - ASSERT(vectorEqual(Vector3(1,2,3),euler)); - ASSERT(equal(euler.getPhi(),1)); - ASSERT(equal(euler.getTheta(),2)); - ASSERT(equal(euler.getPsi(),3)); - - // test dcm ctor - - // test assignment - euler.setPhi(4); - ASSERT(equal(euler.getPhi(),4)); - euler.setTheta(5); - ASSERT(equal(euler.getTheta(),5)); - euler.setPsi(6); - ASSERT(equal(euler.getPsi(),6)); - - printf("PASS\n"); - return 0; + printf("Test EulerAngles\t: "); + EulerAngles euler(1, 2, 3); + + // test ctor + ASSERT(vectorEqual(Vector3(1, 2, 3), euler)); + ASSERT(equal(euler.getPhi(), 1)); + ASSERT(equal(euler.getTheta(), 2)); + ASSERT(equal(euler.getPsi(), 3)); + + // test dcm ctor + + // test assignment + euler.setPhi(4); + ASSERT(equal(euler.getPhi(), 4)); + euler.setTheta(5); + ASSERT(equal(euler.getTheta(), 5)); + euler.setPsi(6); + ASSERT(equal(euler.getPsi(), 6)); + + printf("PASS\n"); + return 0; } } // namespace math diff --git a/apps/systemlib/math/EulerAngles.hpp b/apps/systemlib/math/EulerAngles.hpp index 59e5500ad..399eecfa7 100644 --- a/apps/systemlib/math/EulerAngles.hpp +++ b/apps/systemlib/math/EulerAngles.hpp @@ -41,7 +41,8 @@ #include "Vector.hpp" -namespace math { +namespace math +{ class Quaternion; class Dcm; @@ -49,21 +50,21 @@ 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(); + 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; } + // 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); } + // const accessors + const float &getPhi() const { return (*this)(0); } + const float &getTheta() const { return (*this)(1); } + const float &getPsi() const { return (*this)(2); } }; diff --git a/apps/systemlib/math/Matrix.cpp b/apps/systemlib/math/Matrix.cpp index 08796da89..5e536c3a8 100644 --- a/apps/systemlib/math/Matrix.cpp +++ b/apps/systemlib/math/Matrix.cpp @@ -45,136 +45,149 @@ 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); +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; + 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; + 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; + 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; + 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; + 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; + 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) +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; + if (a.getRows() != b.getRows()) { + printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); + return false; + + } else if (a.getCols() != b.getCols()) { + printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); + return false; + } + + bool ret = true; + + for (size_t i = 0; i < a.getRows(); i++) + for (size_t j = 0; j < a.getCols(); j++) { + if (!equal(a(i, j), b(i, j), eps)) { + printf("element mismatch (%d, %d)\n", i, j); + ret = false; + } + } + + return ret; } - + } // namespace math diff --git a/apps/systemlib/math/Matrix.hpp b/apps/systemlib/math/Matrix.hpp index efa71265c..4d7731653 100644 --- a/apps/systemlib/math/Matrix.hpp +++ b/apps/systemlib/math/Matrix.hpp @@ -45,7 +45,8 @@ #include "generic/Matrix.hpp" #endif -namespace math { +namespace math +{ class Matrix; int matrixTest(); int matrixAddTest(); @@ -54,5 +55,5 @@ int matrixMultTest(); int matrixInvTest(); int matrixDivTest(); 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 diff --git a/apps/systemlib/math/Quaternion.cpp b/apps/systemlib/math/Quaternion.cpp index 79a913af6..12ae72c0e 100644 --- a/apps/systemlib/math/Quaternion.cpp +++ b/apps/systemlib/math/Quaternion.cpp @@ -48,68 +48,68 @@ namespace math { Quaternion::Quaternion() : - Vector(4) + Vector(4) { - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); + setA(1.0f); + setB(0.0f); + setC(0.0f); + setD(0.0f); } Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) + float c, float d) : + Vector(4) { - setA(a); - setB(b); - setC(c); - setD(d); + setA(a); + setB(b); + setC(c); + setD(d); } -Quaternion::Quaternion(const float * data) : - Vector(4,data) +Quaternion::Quaternion(const float *data) : + Vector(4, data) { } -Quaternion::Quaternion(const Vector & v) : - Vector(v) +Quaternion::Quaternion(const Vector &v) : + Vector(v) { } -Quaternion::Quaternion(const Dcm & dcm) : - Vector(4) +Quaternion::Quaternion(const Dcm &dcm) : + Vector(4) { - setA(0.5f*sqrtf(1 + dcm(0,0) + - dcm(1,1) + dcm(2,2))); - setB((dcm(2,1) - dcm(1,2))/ - (4*getA())); - setC((dcm(0,2) - dcm(2,0))/ - (4*getA())); - setD((dcm(1,0) - dcm(0,1))/ - (4*getA())); + setA(0.5f * sqrtf(1 + dcm(0, 0) + + dcm(1, 1) + dcm(2, 2))); + setB((dcm(2, 1) - dcm(1, 2)) / + (4 * getA())); + setC((dcm(0, 2) - dcm(2, 0)) / + (4 * getA())); + setD((dcm(1, 0) - dcm(0, 1)) / + (4 * getA())); } -Quaternion::Quaternion(const EulerAngles & euler) : - Vector(4) +Quaternion::Quaternion(const EulerAngles &euler) : + Vector(4) { - float cosPhi_2 = cosf(euler.getPhi()/2.0f); - float cosTheta_2 = cosf(euler.getTheta()/2.0f); - float cosPsi_2 = cosf(euler.getPsi()/2.0f); - float sinPhi_2 = sinf(euler.getPhi()/2.0f); - float sinTheta_2 = sinf(euler.getTheta()/2.0f); - float sinPsi_2 = sinf(euler.getPsi()/2.0f); - setA(cosPhi_2*cosTheta_2*cosPsi_2 + - sinPhi_2*sinTheta_2*sinPsi_2); - setB(sinPhi_2*cosTheta_2*cosPsi_2 - - cosPhi_2*sinTheta_2*sinPsi_2); - setC(cosPhi_2*sinTheta_2*cosPsi_2 + - sinPhi_2*cosTheta_2*sinPsi_2); - setD(cosPhi_2*cosTheta_2*sinPsi_2 + - sinPhi_2*sinTheta_2*cosPsi_2); + float cosPhi_2 = cosf(euler.getPhi() / 2.0f); + float cosTheta_2 = cosf(euler.getTheta() / 2.0f); + float cosPsi_2 = cosf(euler.getPsi() / 2.0f); + float sinPhi_2 = sinf(euler.getPhi() / 2.0f); + float sinTheta_2 = sinf(euler.getTheta() / 2.0f); + float sinPsi_2 = sinf(euler.getPsi() / 2.0f); + setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + setD(cosPhi_2 * cosTheta_2 * sinPsi_2 + + sinPhi_2 * sinTheta_2 * cosPsi_2); } -Quaternion::Quaternion(const Quaternion & right) : - Vector(right) +Quaternion::Quaternion(const Quaternion &right) : + Vector(right) { } @@ -117,64 +117,65 @@ Quaternion::~Quaternion() { } -Vector Quaternion::derivative(const Vector & w) +Vector Quaternion::derivative(const Vector &w) { #ifdef QUATERNION_ASSERT - ASSERT(w.getRows()==3); + 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; + float dataQ[] = { + getA(), -getB(), -getC(), -getD(), + getB(), getA(), -getD(), getC(), + getC(), getD(), getA(), -getB(), + getD(), -getC(), getB(), getA() + }; + Vector v(4); + v(0) = 0.0f; + v(1) = w(0); + v(2) = w(1); + v(3) = w(2); + Matrix Q(4, 4, dataQ); + return Q * v * 0.5f; } int __EXPORT quaternionTest() { - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(),1)); - ASSERT(equal(q.getB(),0)); - ASSERT(equal(q.getC(),0)); - ASSERT(equal(q.getD(),0)); - // test float ctor - q = Quaternion(0,1,0,0); - ASSERT(equal(q.getA(),0)); - ASSERT(equal(q.getB(),1)); - ASSERT(equal(q.getC(),0)); - ASSERT(equal(q.getD(),0)); - // test euler ctor - q = Quaternion(EulerAngles(0,0,0)); - ASSERT(equal(q.getA(),1)); - ASSERT(equal(q.getB(),0)); - ASSERT(equal(q.getC(),0)); - ASSERT(equal(q.getD(),0)); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(equal(q.getA(),1)); - ASSERT(equal(q.getB(),0)); - ASSERT(equal(q.getC(),0)); - ASSERT(equal(q.getD(),0)); - // TODO test derivative - // test accessors - q.setA(0.1); - q.setB(0.2); - q.setC(0.3); - q.setD(0.4); - ASSERT(equal(q.getA(),0.1)); - ASSERT(equal(q.getB(),0.2)); - ASSERT(equal(q.getC(),0.3)); - ASSERT(equal(q.getD(),0.4)); - printf("PASS\n"); - return 0; + printf("Test Quaternion\t\t: "); + // test default ctor + Quaternion q; + ASSERT(equal(q.getA(), 1)); + ASSERT(equal(q.getB(), 0)); + ASSERT(equal(q.getC(), 0)); + ASSERT(equal(q.getD(), 0)); + // test float ctor + q = Quaternion(0, 1, 0, 0); + ASSERT(equal(q.getA(), 0)); + ASSERT(equal(q.getB(), 1)); + ASSERT(equal(q.getC(), 0)); + ASSERT(equal(q.getD(), 0)); + // test euler ctor + q = Quaternion(EulerAngles(0, 0, 0)); + ASSERT(equal(q.getA(), 1)); + ASSERT(equal(q.getB(), 0)); + ASSERT(equal(q.getC(), 0)); + ASSERT(equal(q.getD(), 0)); + // test dcm ctor + q = Quaternion(Dcm()); + ASSERT(equal(q.getA(), 1)); + ASSERT(equal(q.getB(), 0)); + ASSERT(equal(q.getC(), 0)); + ASSERT(equal(q.getD(), 0)); + // TODO test derivative + // test accessors + q.setA(0.1); + q.setB(0.2); + q.setC(0.3); + q.setD(0.4); + ASSERT(equal(q.getA(), 0.1)); + ASSERT(equal(q.getB(), 0.2)); + ASSERT(equal(q.getC(), 0.3)); + ASSERT(equal(q.getD(), 0.4)); + printf("PASS\n"); + return 0; } } // namespace math diff --git a/apps/systemlib/math/Quaternion.hpp b/apps/systemlib/math/Quaternion.hpp index 334b0dd03..4b4e959d8 100644 --- a/apps/systemlib/math/Quaternion.hpp +++ b/apps/systemlib/math/Quaternion.hpp @@ -42,7 +42,8 @@ #include "Vector.hpp" #include "Matrix.hpp" -namespace math { +namespace math +{ class Dcm; class EulerAngles; @@ -51,62 +52,62 @@ class __EXPORT Quaternion : public Vector { public: - /** - * default ctor - */ - Quaternion(); - - /** - * ctor from floats - */ - Quaternion(float a, float b, float c, float d); - - /** - * ctor from data - */ - Quaternion(const float * data); - - /** - * ctor from Vector - */ - Quaternion(const Vector & v); - - /** - * ctor from EulerAngles - */ - Quaternion(const EulerAngles & euler); - - /** - * ctor from Dcm - */ - Quaternion(const Dcm & dcm); - - /** - * deep copy ctor - */ - Quaternion(const Quaternion & right); - - /** - * dtor - */ - virtual ~Quaternion(); - - /** - * derivative - */ - Vector derivative(const Vector & w); - - /** - * accessors - */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float & getA() const { return (*this)(0); } - const float & getB() const { return (*this)(1); } - const float & getC() const { return (*this)(2); } - const float & getD() const { return (*this)(3); } + /** + * default ctor + */ + Quaternion(); + + /** + * ctor from floats + */ + Quaternion(float a, float b, float c, float d); + + /** + * ctor from data + */ + Quaternion(const float *data); + + /** + * ctor from Vector + */ + Quaternion(const Vector &v); + + /** + * ctor from EulerAngles + */ + Quaternion(const EulerAngles &euler); + + /** + * ctor from Dcm + */ + Quaternion(const Dcm &dcm); + + /** + * deep copy ctor + */ + Quaternion(const Quaternion &right); + + /** + * dtor + */ + virtual ~Quaternion(); + + /** + * derivative + */ + Vector derivative(const Vector &w); + + /** + * accessors + */ + void setA(float a) { (*this)(0) = a; } + void setB(float b) { (*this)(1) = b; } + void setC(float c) { (*this)(2) = c; } + void setD(float d) { (*this)(3) = d; } + const float &getA() const { return (*this)(0); } + const float &getB() const { return (*this)(1); } + const float &getC() const { return (*this)(2); } + const float &getD() const { return (*this)(3); } }; int __EXPORT quaternionTest(); diff --git a/apps/systemlib/math/Vector.cpp b/apps/systemlib/math/Vector.cpp index 5bdbc9168..c039a758b 100644 --- a/apps/systemlib/math/Vector.cpp +++ b/apps/systemlib/math/Vector.cpp @@ -44,56 +44,57 @@ namespace math { -static const float data_testA[] = {1,3}; -static const float data_testB[] = {4,1}; +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); +static Vector testA(2, data_testA); +static Vector testB(2, data_testB); int __EXPORT vectorTest() { - vectorAddTest(); - vectorSubTest(); - return 0; + 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; + 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; + 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) +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; + if (a.getRows() != b.getRows()) { + printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); + return false; + } + + bool ret = true; + + for (size_t i = 0; i < a.getRows(); i++) { + if (!equal(a(i), b(i), eps)) { + printf("element mismatch (%d)\n", i); + ret = false; + } + } + + return ret; } } // namespace math diff --git a/apps/systemlib/math/Vector.hpp b/apps/systemlib/math/Vector.hpp index 0c5ac2b19..773bd5032 100644 --- a/apps/systemlib/math/Vector.hpp +++ b/apps/systemlib/math/Vector.hpp @@ -45,10 +45,11 @@ #include "generic/Vector.hpp" #endif -namespace math { +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); +bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); } // math diff --git a/apps/systemlib/math/Vector3.cpp b/apps/systemlib/math/Vector3.cpp index a30b6d198..edba663cb 100644 --- a/apps/systemlib/math/Vector3.cpp +++ b/apps/systemlib/math/Vector3.cpp @@ -45,28 +45,28 @@ namespace math { Vector3::Vector3() : - Vector(3) + Vector(3) { } -Vector3::Vector3(const Vector & right) : - Vector(right) +Vector3::Vector3(const Vector &right) : + Vector(right) { #ifdef VECTOR_ASSERT - ASSERT(right.getRows()==3); + ASSERT(right.getRows() == 3); #endif } Vector3::Vector3(float x, float y, float z) : - Vector(3) + Vector(3) { - setX(x); - setY(y); - setZ(z); + setX(x); + setY(y); + setZ(z); } -Vector3::Vector3(const float * data) : - Vector(3,data) +Vector3::Vector3(const float *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 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 &a = *this; + Vector3 result; + result(0) = a(1) * b(2) - a(2) * b(1); + result(1) = a(2) * b(0) - a(0) * b(2); + result(2) = a(0) * b(1) - a(1) * b(0); + return result; } int __EXPORT vector3Test() { - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1,2,3); - ASSERT(equal(v(0),1)); - ASSERT(equal(v(1),2)); - ASSERT(equal(v(2),3)); - printf("PASS\n"); - return 0; + printf("Test Vector3\t\t: "); + // test float ctor + Vector3 v(1, 2, 3); + ASSERT(equal(v(0), 1)); + ASSERT(equal(v(1), 2)); + ASSERT(equal(v(2), 3)); + printf("PASS\n"); + return 0; } } // namespace math diff --git a/apps/systemlib/math/Vector3.hpp b/apps/systemlib/math/Vector3.hpp index 48dfd5b22..8c36ac134 100644 --- a/apps/systemlib/math/Vector3.hpp +++ b/apps/systemlib/math/Vector3.hpp @@ -45,25 +45,25 @@ namespace math { class __EXPORT Vector3 : - public Vector + 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); + Vector3(); + Vector3(const Vector &right); + Vector3(float x, float y, float z); + Vector3(const float *data); + virtual ~Vector3(); + Vector3 cross(const Vector3 &b); - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float & getX() const { return (*this)(0); } - const float & getY() const { return (*this)(1); } - const float & getZ() const { return (*this)(2); } + /** + * accessors + */ + void setX(float x) { (*this)(0) = x; } + void setY(float y) { (*this)(1) = y; } + void setZ(float z) { (*this)(2) = z; } + const float &getX() const { return (*this)(0); } + const float &getY() const { return (*this)(1); } + const float &getZ() const { return (*this)(2); } }; int __EXPORT vector3Test(); diff --git a/apps/systemlib/math/arm/Matrix.hpp b/apps/systemlib/math/arm/Matrix.hpp index a94980872..a95b5a2b0 100644 --- a/apps/systemlib/math/arm/Matrix.hpp +++ b/apps/systemlib/math/arm/Matrix.hpp @@ -56,257 +56,237 @@ namespace math { -class __EXPORT Matrix { +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) - { + // 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()); + 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) - { + + 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()); + ASSERT(i < getRows()); + ASSERT(j < getCols()); #endif - return getData()[i*getCols() + j]; - } - inline const float & operator()(size_t i, size_t j) const - { + 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()); + ASSERT(i < getRows()); + ASSERT(j < getCols()); #endif - return getData()[i*getCols() + j]; - } - // output - inline void print() const - { - for (size_t i=0; i<getRows(); i++) - { - for (size_t j=0; j<getCols(); j++) - { - float sig; - int exp; - float num = (*this)(i,j); - float2SigExp(num,sig,exp); - printf ("%6.3fe%03.3d,", (double)sig, exp); - } - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix & right) const - { - for (size_t i=0; i<getRows(); i++) - { - for (size_t j=0; j<getCols(); j++) - { - if (fabsf((*this)(i,j)-right(i,j)) > 1e-30f) - return false; - } - } - return true; - } - // scalar ops - inline Matrix operator+(float right) const - { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(),right, - (float *)result.getData(),getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const - { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(),-right, - (float *)result.getData(),getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const - { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix,right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const - { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix,1.0f/right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector & right) const - { + return getData()[i * getCols() + j]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + float sig; + int exp; + float num = (*this)(i, j); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + } + // boolean ops + inline bool operator==(const Matrix &right) const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) + return false; + } + } + + return true; + } + // scalar ops + inline Matrix operator+(float right) const { + Matrix result(getRows(), getCols()); + arm_offset_f32((float *)getData(), right, + (float *)result.getData(), getRows()*getCols()); + return result; + } + inline Matrix operator-(float right) const { + Matrix result(getRows(), getCols()); + arm_offset_f32((float *)getData(), -right, + (float *)result.getData(), getRows()*getCols()); + return result; + } + inline Matrix operator*(float right) const { + Matrix result(getRows(), getCols()); + arm_mat_scale_f32(&_matrix, right, + &(result._matrix)); + return result; + } + inline Matrix operator/(float right) const { + Matrix result(getRows(), getCols()); + arm_mat_scale_f32(&_matrix, 1.0f / right, + &(result._matrix)); + return result; + } + // vector ops + inline Vector operator*(const Vector &right) const { #ifdef MATRIX_ASSERT - ASSERT(getCols()==right.getRows()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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; - } + 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; } + 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; + arm_matrix_instance_f32 _matrix; }; } // namespace math diff --git a/apps/systemlib/math/arm/Vector.hpp b/apps/systemlib/math/arm/Vector.hpp index b12f112f6..c1e13f29a 100644 --- a/apps/systemlib/math/arm/Vector.hpp +++ b/apps/systemlib/math/arm/Vector.hpp @@ -55,186 +55,166 @@ namespace math { -class __EXPORT Vector { +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) - { + // 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()); + ASSERT(getRows() == right.getRows()); #endif - if (this != &right) - { - memcpy(getData(),right.getData(), - right.getSize()); - } - return *this; - } - // element accessors - inline float& operator()(size_t i) - { + + 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()); + ASSERT(i < getRows()); #endif - return getData()[i]; - } - inline const float& operator()(size_t i) const - { + return getData()[i]; + } + inline const float &operator()(size_t i) const { #ifdef VECTOR_ASSERT - ASSERT(i<getRows()); + ASSERT(i < getRows()); #endif - return getData()[i]; - } - // output - inline void print() const - { - for (size_t i=0; i<getRows(); i++) - { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num,sig,exp); - printf ("%6.3fe%03.3d,", (double)sig, exp); - } - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector & right) const - { - for (size_t i=0; i<getRows(); i++) - { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - return true; - } - // scalar ops - inline Vector operator+(float right) const - { - Vector result(getRows()); - arm_offset_f32((float*)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const - { - Vector result(getRows()); - arm_offset_f32((float*)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const - { - Vector result(getRows()); - arm_scale_f32((float*)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const - { - Vector result(getRows()); - arm_scale_f32((float*)getData(), - 1.0f/right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector & right) const - { + return getData()[i]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + float sig; + int exp; + float num = (*this)(i); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + // boolean ops + inline bool operator==(const Vector &right) const { + for (size_t i = 0; i < getRows(); i++) { + if (fabsf(((*this)(i) - right(i))) > 1e-30f) + return false; + } + + return true; + } + // scalar ops + inline Vector operator+(float right) const { + Vector result(getRows()); + arm_offset_f32((float *)getData(), + right, result.getData(), + getRows()); + return result; + } + inline Vector operator-(float right) const { + Vector result(getRows()); + arm_offset_f32((float *)getData(), + -right, result.getData(), + getRows()); + return result; + } + inline Vector operator*(float right) const { + Vector result(getRows()); + arm_scale_f32((float *)getData(), + right, result.getData(), + getRows()); + return result; + } + inline Vector operator/(float right) const { + Vector result(getRows()); + arm_scale_f32((float *)getData(), + 1.0f / right, result.getData(), + getRows()); + return result; + } + // vector ops + inline Vector operator+(const Vector &right) const { #ifdef VECTOR_ASSERT - ASSERT(getRows()==right.getRows()); + 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 - { + 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()); + ASSERT(getRows() == right.getRows()); #endif - Vector result(getRows()); - arm_sub_f32((float*)getData(), - (float*)right.getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector & right) - { - float result = 0; - arm_dot_prod_f32((float*)getData(), - (float*)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() - { - return sqrtf(dot(*this)); - } - inline Vector unit() - { - return (*this)/norm(); - } - inline static Vector zero(size_t rows) - { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float & val) - { - for (size_t i=0;i<getRows();i++) - { - (*this)(i) = val; - } - } - inline void set(const float * data) - { - memcpy(getData(),data,getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float * getData() const { return _data; } + Vector result(getRows()); + arm_sub_f32((float *)getData(), + (float *)right.getData(), + result.getData(), + getRows()); + return result; + } + // other functions + inline float dot(const Vector &right) { + float result = 0; + arm_dot_prod_f32((float *)getData(), + (float *)right.getData(), + getRows(), + &result); + return result; + } + inline float norm() { + return sqrtf(dot(*this)); + } + inline Vector unit() { + return (*this) / norm(); + } + inline static Vector zero(size_t rows) { + Vector result(rows); + // calloc returns zeroed memory + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + (*this)(i) = val; + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } + inline const float *getData() const { return _data; } protected: - inline size_t getSize() const { return sizeof(float)*getRows(); } - inline float * getData() { return _data; } - inline void setData(float * data) { _data = data; } + 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; + size_t _rows; + float *_data; }; } // math diff --git a/apps/systemlib/math/generic/Matrix.hpp b/apps/systemlib/math/generic/Matrix.hpp index fd6c8ba51..d10208a1e 100644 --- a/apps/systemlib/math/generic/Matrix.hpp +++ b/apps/systemlib/math/generic/Matrix.hpp @@ -53,395 +53,385 @@ namespace math { -class __EXPORT Matrix { +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) - { + // 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()); + 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) - { + + 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()); + ASSERT(i < getRows()); + ASSERT(j < getCols()); #endif - return getData()[i*getCols() + j]; - } - inline const float & operator()(size_t i, size_t j) const - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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); - } - } - } + 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); + //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; - } + + // 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; } + 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; + size_t _rows; + size_t _cols; + float *_data; }; } // namespace math diff --git a/apps/systemlib/math/generic/Vector.hpp b/apps/systemlib/math/generic/Vector.hpp index 4d867f360..01cfa8bc0 100644 --- a/apps/systemlib/math/generic/Vector.hpp +++ b/apps/systemlib/math/generic/Vector.hpp @@ -51,190 +51,177 @@ namespace math { -class __EXPORT Vector { +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) - { + // 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()); + ASSERT(getRows() == right.getRows()); #endif - if (this != &right) - { - memcpy(getData(),right.getData(), - right.getSize()); - } - return *this; - } - // element accessors - inline float& operator()(size_t i) - { + + 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()); + ASSERT(i < getRows()); #endif - return getData()[i]; - } - inline const float& operator()(size_t i) const - { + return getData()[i]; + } + inline const float &operator()(size_t i) const { #ifdef VECTOR_ASSERT - ASSERT(i<getRows()); + 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 - { + 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()); + 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 - { + 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()); + ASSERT(getRows() == right.getRows()); #endif - Vector result(getRows()); - for (size_t i=0; i<getRows(); i++) - { - result(i) = (*this)(i) - right(i); - } - return result; - } - // other functions - inline float dot(const Vector & right) - { - float result = 0; - for (size_t i=0; i<getRows(); i++) - { - result += (*this)(i)*(*this)(i); - } - return result; - } - inline float norm() - { - return sqrtf(dot(*this)); - } - inline Vector unit() - { - return (*this)/norm(); - } - inline static Vector zero(size_t rows) - { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float & val) - { - for (size_t i=0;i<getRows();i++) - { - (*this)(i) = val; - } - } - inline void set(const float * data) - { - memcpy(getData(),data,getSize()); - } - inline size_t getRows() const { return _rows; } + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right(i); + } + + return result; + } + // other functions + inline float dot(const Vector &right) { + float result = 0; + + for (size_t i = 0; i < getRows(); i++) { + result += (*this)(i) * (*this)(i); + } + + return result; + } + inline float norm() { + return sqrtf(dot(*this)); + } + inline Vector unit() { + return (*this) / norm(); + } + inline static Vector zero(size_t rows) { + Vector result(rows); + // calloc returns zeroed memory + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + (*this)(i) = val; + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } protected: - inline size_t getSize() const { return sizeof(float)*getRows(); } - inline float * getData() { return _data; } - inline const float * getData() const { return _data; } - inline void setData(float * data) { _data = data; } + 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; + size_t _rows; + float *_data; }; } // math diff --git a/apps/systemlib/test/test.cpp b/apps/systemlib/test/test.cpp index aa00e66fa..f5db76d70 100644 --- a/apps/systemlib/test/test.cpp +++ b/apps/systemlib/test/test.cpp @@ -44,45 +44,50 @@ bool __EXPORT equal(float a, float b, float epsilon) { - float diff = fabsf(a-b); - if (diff>epsilon) - { - printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); - return false; - } - else return true; + float diff = fabsf(a - b); + + if (diff > epsilon) { + printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + + } else return true; } void __EXPORT float2SigExp( - const float & num, - float & sig, - int & exp) + const float &num, + float &sig, + int &exp) { - if (isnan(num) || isinf(num)) - { - sig = 0.0f; - exp = -99; - return; - } - if (fabsf(num) < 1.0e-38f) - { - sig = 0; - exp = 0; - return; - } - exp = log10f(fabsf(num)); - if (exp>0) { - exp = ceil(exp); - } else { - exp = floor(exp); - } - sig = num; - // cheap power since it is integer - if (exp>0) { - for (int i=0;i<abs(exp);i++) sig /= 10; - } else { - for (int i=0;i<abs(exp);i++) sig *= 10; - } + if (isnan(num) || isinf(num)) { + sig = 0.0f; + exp = -99; + return; + } + + if (fabsf(num) < 1.0e-38f) { + sig = 0; + exp = 0; + return; + } + + exp = log10f(fabsf(num)); + + if (exp > 0) { + exp = ceil(exp); + + } else { + exp = floor(exp); + } + + sig = num; + + // cheap power since it is integer + if (exp > 0) { + for (int i = 0; i < abs(exp); i++) sig /= 10; + + } else { + for (int i = 0; i < abs(exp); i++) sig *= 10; + } } diff --git a/apps/systemlib/test/test.hpp b/apps/systemlib/test/test.hpp index 860758e7d..841c42144 100644 --- a/apps/systemlib/test/test.hpp +++ b/apps/systemlib/test/test.hpp @@ -45,6 +45,6 @@ bool equal(float a, float b, float eps = 1e-5); void float2SigExp( - const float & num, - float & sig, - int & exp); + const float &num, + float &sig, + int &exp); |