aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rwxr-xr-xTools/fix_code_style_ubuntu.sh19
-rw-r--r--apps/systemlib/math/Dcm.cpp120
-rw-r--r--apps/systemlib/math/Dcm.hpp53
-rw-r--r--apps/systemlib/math/EulerAngles.cpp102
-rw-r--r--apps/systemlib/math/EulerAngles.hpp29
-rw-r--r--apps/systemlib/math/Matrix.cpp223
-rw-r--r--apps/systemlib/math/Matrix.hpp5
-rw-r--r--apps/systemlib/math/Quaternion.cpp189
-rw-r--r--apps/systemlib/math/Quaternion.hpp115
-rw-r--r--apps/systemlib/math/Vector.cpp71
-rw-r--r--apps/systemlib/math/Vector.hpp5
-rw-r--r--apps/systemlib/math/Vector3.cpp50
-rw-r--r--apps/systemlib/math/Vector3.hpp32
-rw-r--r--apps/systemlib/math/arm/Matrix.hpp440
-rw-r--r--apps/systemlib/math/arm/Vector.hpp314
-rw-r--r--apps/systemlib/math/generic/Matrix.hpp722
-rw-r--r--apps/systemlib/math/generic/Vector.hpp329
-rw-r--r--apps/systemlib/test/test.cpp75
-rw-r--r--apps/systemlib/test/test.hpp6
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);
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-29 18:51:41 +0000