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

Merge commit 'd9491b20cc5fc8b683eb0f60a50da6b322b55e57' into local/mathlib

Browse Source
This commit is contained in:
px4dev
2013-01-06 12:57:33 -08:00
parent 95aa959571 d9491b20cc
commit 98a53e85d6
19 changed files with 1432 additions and 1453 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/fix_code_style_ubuntu.sh
Executable
+19
View File
@@ -0,0 +1,19 @@
#!/bin/sh
astyle \
--style=linux \
--indent=force-tab=8 \
--indent-cases \
--indent-preprocessor \
--break-blocks=all \
--pad-oper \
--pad-header \
--unpad-paren \
--keep-one-line-blocks \
--keep-one-line-statements \
--align-pointer=name \
--suffix=none \
--lineend=linux \
$*
#--ignore-exclude-errors-x \
#--exclude=EASTL \
#--align-reference=name \
apps/systemlib/math/Dcm.hpp
+2 -1
View File
@@ -42,7 +42,8 @@
#include "Vector.hpp" #include "Vector.hpp"
#include "Matrix.hpp" #include "Matrix.hpp"
namespace math { namespace math
{
class Quaternion; class Quaternion;
class EulerAngles; class EulerAngles;
apps/systemlib/math/EulerAngles.cpp
+6 -8
View File
@@ -73,20 +73,18 @@ EulerAngles::EulerAngles(const Dcm & dcm) :
Vector(3) Vector(3)
{ {
setTheta(asinf(-dcm(2, 0))); setTheta(asinf(-dcm(2, 0)));
if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f)
{ if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f); setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1), setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) + getPhi()); dcm(0, 2) + dcm(1, 1)) + getPhi());
}
else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
{
setPhi(0.0f); setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1), setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) - getPhi()); dcm(0, 2) + dcm(1, 1)) - getPhi());
}
else } else {
{
setPhi(atan2f(dcm(2, 1), dcm(2, 2))); setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
setPsi(atan2f(dcm(1, 0), dcm(0, 0))); setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
} }
apps/systemlib/math/EulerAngles.hpp
+2 -1
View File
@@ -41,7 +41,8 @@
#include "Vector.hpp" #include "Vector.hpp"
namespace math { namespace math
{
class Quaternion; class Quaternion;
class Dcm; class Dcm;
apps/systemlib/math/Matrix.cpp
+48 -35
View File
@@ -45,38 +45,44 @@
namespace math namespace math
{ {
static const float data_testA[] = static const float data_testA[] = {
{1,2,3, 1, 2, 3,
4,5,6}; 4, 5, 6
};
static Matrix testA(2, 3, data_testA); static Matrix testA(2, 3, data_testA);
static const float data_testB[] = static const float data_testB[] = {
{0,1,3, 0, 1, 3,
7,-1,2}; 7, -1, 2
};
static Matrix testB(2, 3, data_testB); static Matrix testB(2, 3, data_testB);
static const float data_testC[] = static const float data_testC[] = {
{0,1, 0, 1,
2, 1, 2, 1,
3,2}; 3, 2
};
static Matrix testC(3, 2, data_testC); static Matrix testC(3, 2, data_testC);
static const float data_testD[] = static const float data_testD[] = {
{0,1,2, 0, 1, 2,
2, 1, 4, 2, 1, 4,
5,2,0}; 5, 2, 0
};
static Matrix testD(3, 3, data_testD); static Matrix testD(3, 3, data_testD);
static const float data_testE[] = static const float data_testE[] = {
{1,-1,2, 1, -1, 2,
0, 2, 3, 0, 2, 3,
2,-1,1}; 2, -1, 1
};
static Matrix testE(3, 3, data_testE); static Matrix testE(3, 3, data_testE);
static const float data_testF[] = static const float data_testF[] = {
{3.777e006f, 2.915e007f, 0.000e000f, 3.777e006f, 2.915e007f, 0.000e000f,
2.938e007f, 2.267e008f, 0.000e000f, 2.938e007f, 2.267e008f, 0.000e000f,
0.000e000f, 0.000e000f, 6.033e008f}; 0.000e000f, 0.000e000f, 6.033e008f
};
static Matrix testF(3, 3, data_testF); static Matrix testF(3, 3, data_testF);
int __EXPORT matrixTest() int __EXPORT matrixTest()
@@ -93,9 +99,10 @@ int matrixAddTest()
{ {
printf("Test Matrix Add\t\t: "); printf("Test Matrix Add\t\t: ");
Matrix r = testA + testB; Matrix r = testA + testB;
float data_test[] = float data_test[] = {
{ 1.0f, 3.0f, 6.0f, 1.0f, 3.0f, 6.0f,
11.0f, 4.0f, 8.0f}; 11.0f, 4.0f, 8.0f
};
ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
@@ -105,9 +112,10 @@ int matrixSubTest()
{ {
printf("Test Matrix Sub\t\t: "); printf("Test Matrix Sub\t\t: ");
Matrix r = testA - testB; Matrix r = testA - testB;
float data_test[] = float data_test[] = {
{ 1.0f, 1.0f, 0.0f, 1.0f, 1.0f, 0.0f,
-3.0f, 6.0f, 4.0f}; -3.0f, 6.0f, 4.0f
};
ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
@@ -117,10 +125,11 @@ int matrixMultTest()
{ {
printf("Test Matrix Mult\t: "); printf("Test Matrix Mult\t: ");
Matrix r = testC * testB; Matrix r = testC * testB;
float data_test[] = float data_test[] = {
{ 7.0f, -1.0f, 2.0f, 7.0f, -1.0f, 2.0f,
7.0f, 1.0f, 8.0f, 7.0f, 1.0f, 8.0f,
14.0f, 1.0f, 13.0f}; 14.0f, 1.0f, 13.0f
};
ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
@@ -131,10 +140,11 @@ int matrixInvTest()
printf("Test Matrix Inv\t\t: "); printf("Test Matrix Inv\t\t: ");
Matrix origF = testF; Matrix origF = testF;
Matrix r = testF.inverse(); Matrix r = testF.inverse();
float data_test[] = float data_test[] = {
{ -0.0012518f, 0.0001610f, 0.0000000f, -0.0012518f, 0.0001610f, 0.0000000f,
0.0001622f, -0.0000209f, 0.0000000f, 0.0001622f, -0.0000209f, 0.0000000f,
0.0000000f, 0.0000000f, 1.6580e-9f}; 0.0000000f, 0.0000000f, 1.6580e-9f
};
ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
// make sure F in unchanged // make sure F in unchanged
ASSERT(matrixEqual(origF, testF)); ASSERT(matrixEqual(origF, testF));
@@ -149,7 +159,8 @@ int matrixDivTest()
float data_test[] = { float data_test[] = {
0.2222222f, 0.5555556f, -0.1111111f, 0.2222222f, 0.5555556f, -0.1111111f,
0.0f, 1.0f, 1.0, 0.0f, 1.0f, 1.0,
-4.1111111f, 1.2222222f, 4.5555556f}; -4.1111111f, 1.2222222f, 4.5555556f
};
ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
printf("PASS\n"); printf("PASS\n");
return 0; return 0;
@@ -160,20 +171,22 @@ bool matrixEqual(const Matrix & a, const Matrix & b, float eps)
if (a.getRows() != b.getRows()) { if (a.getRows() != b.getRows()) {
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
return false; return false;
} else if (a.getCols() != b.getCols()) { } else if (a.getCols() != b.getCols()) {
printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
return false; return false;
} }
bool ret = true; bool ret = true;
for (size_t i = 0; i < a.getRows(); i++) for (size_t i = 0; i < a.getRows(); i++)
for (size_t j =0;j<a.getCols();j++) for (size_t j = 0; j < a.getCols(); j++) {
{ if (!equal(a(i, j), b(i, j), eps)) {
if (!equal(a(i,j),b(i,j),eps))
{
printf("element mismatch (%d, %d)\n", i, j); printf("element mismatch (%d, %d)\n", i, j);
ret = false; ret = false;
} }
} }
return ret; return ret;
} }
apps/systemlib/math/Matrix.hpp
+2 -1
View File
@@ -45,7 +45,8 @@
#include "generic/Matrix.hpp" #include "generic/Matrix.hpp"
#endif #endif
namespace math { namespace math
{
class Matrix; class Matrix;
int matrixTest(); int matrixTest();
int matrixAddTest(); int matrixAddTest();
apps/systemlib/math/Quaternion.cpp
+4 -3
View File
@@ -122,11 +122,12 @@ Vector Quaternion::derivative(const Vector & w)
#ifdef QUATERNION_ASSERT #ifdef QUATERNION_ASSERT
ASSERT(w.getRows() == 3); ASSERT(w.getRows() == 3);
#endif #endif
float dataQ[] = float dataQ[] = {
{getA(), -getB(), -getC(), -getD(), getA(), -getB(), -getC(), -getD(),
getB(), getA(), -getD(), getC(), getB(), getA(), -getD(), getC(),
getC(), getD(), getA(), -getB(), getC(), getD(), getA(), -getB(),
getD(), -getC(), getB(), getA()}; getD(), -getC(), getB(), getA()
};
Vector v(4); Vector v(4);
v(0) = 0.0f; v(0) = 0.0f;
v(1) = w(0); v(1) = w(0);
apps/systemlib/math/Quaternion.hpp
+2 -1
View File
@@ -42,7 +42,8 @@
#include "Vector.hpp" #include "Vector.hpp"
#include "Matrix.hpp" #include "Matrix.hpp"
namespace math { namespace math
{
class Dcm; class Dcm;
class EulerAngles; class EulerAngles;
apps/systemlib/math/Vector.cpp
+5 -4
View File
@@ -84,15 +84,16 @@ bool vectorEqual(const Vector & a, const Vector & b, float eps)
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
return false; return false;
} }
bool ret = true; bool ret = true;
for (size_t i=0;i<a.getRows();i++)
{ for (size_t i = 0; i < a.getRows(); i++) {
if (!equal(a(i),b(i),eps)) if (!equal(a(i), b(i), eps)) {
{
printf("element mismatch (%d)\n", i); printf("element mismatch (%d)\n", i);
ret = false; ret = false;
} }
} }
return ret; return ret;
} }
apps/systemlib/math/Vector.hpp
+2 -1
View File
@@ -45,7 +45,8 @@
#include "generic/Vector.hpp" #include "generic/Vector.hpp"
#endif #endif
namespace math { namespace math
{
class Vector; class Vector;
int __EXPORT vectorTest(); int __EXPORT vectorTest();
int __EXPORT vectorAddTest(); int __EXPORT vectorAddTest();
apps/systemlib/math/arm/Matrix.hpp
+39 -59
View File
@@ -56,33 +56,30 @@
namespace math namespace math
{ {
class __EXPORT Matrix { class __EXPORT Matrix
{
public: public:
// constructor // constructor
Matrix(size_t rows, size_t cols) : Matrix(size_t rows, size_t cols) :
_matrix() _matrix() {
{
arm_mat_init_f32(&_matrix, arm_mat_init_f32(&_matrix,
rows, cols, rows, cols,
(float *)calloc(rows * cols, sizeof(float))); (float *)calloc(rows * cols, sizeof(float)));
} }
Matrix(size_t rows, size_t cols, const float *data) : Matrix(size_t rows, size_t cols, const float *data) :
_matrix() _matrix() {
{
arm_mat_init_f32(&_matrix, arm_mat_init_f32(&_matrix,
rows, cols, rows, cols,
(float *)malloc(rows * cols * sizeof(float))); (float *)malloc(rows * cols * sizeof(float)));
memcpy(getData(), data, getSize()); memcpy(getData(), data, getSize());
} }
// deconstructor // deconstructor
virtual ~Matrix() virtual ~Matrix() {
{
delete [] _matrix.pData; delete [] _matrix.pData;
} }
// copy constructor (deep) // copy constructor (deep)
Matrix(const Matrix &right) : Matrix(const Matrix &right) :
_matrix() _matrix() {
{
arm_mat_init_f32(&_matrix, arm_mat_init_f32(&_matrix,
right.getRows(), right.getCols(), right.getRows(), right.getCols(),
(float *)malloc(right.getRows()* (float *)malloc(right.getRows()*
@@ -91,30 +88,28 @@ public:
getSize()); getSize());
} }
// assignment // assignment
inline Matrix & operator=(const Matrix & right) inline Matrix &operator=(const Matrix &right) {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols() == right.getCols()); ASSERT(getCols() == right.getCols());
#endif #endif
if (this != &right)
{ if (this != &right) {
memcpy(getData(), right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
return *this; return *this;
} }
// element accessors // element accessors
inline float & operator()(size_t i, size_t j) inline float &operator()(size_t i, size_t j) {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(i < getRows()); ASSERT(i < getRows());
ASSERT(j < getCols()); ASSERT(j < getCols());
#endif #endif
return getData()[i * getCols() + j]; return getData()[i * getCols() + j];
} }
inline const float & operator()(size_t i, size_t j) const inline const float &operator()(size_t i, size_t j) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(i < getRows()); ASSERT(i < getRows());
ASSERT(j < getCols()); ASSERT(j < getCols());
@@ -122,66 +117,57 @@ public:
return getData()[i * getCols() + j]; return getData()[i * getCols() + j];
} }
// output // output
inline void print() const inline void print() const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++) for (size_t j = 0; j < getCols(); j++) {
{
for (size_t j=0; j<getCols(); j++)
{
float sig; float sig;
int exp; int exp;
float num = (*this)(i, j); float num = (*this)(i, j);
float2SigExp(num, sig, exp); float2SigExp(num, sig, exp);
printf("%6.3fe%03.3d,", (double)sig, exp); printf("%6.3fe%03.3d,", (double)sig, exp);
} }
printf("\n"); printf("\n");
} }
} }
// boolean ops // boolean ops
inline bool operator==(const Matrix & right) const inline bool operator==(const Matrix &right) const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++) for (size_t j = 0; j < getCols(); j++) {
{
for (size_t j=0; j<getCols(); j++)
{
if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
return false; return false;
} }
} }
return true; return true;
} }
// scalar ops // scalar ops
inline Matrix operator+(float right) const inline Matrix operator+(float right) const {
{
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
arm_offset_f32((float *)getData(), right, arm_offset_f32((float *)getData(), right,
(float *)result.getData(), getRows()*getCols()); (float *)result.getData(), getRows()*getCols());
return result; return result;
} }
inline Matrix operator-(float right) const inline Matrix operator-(float right) const {
{
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
arm_offset_f32((float *)getData(), -right, arm_offset_f32((float *)getData(), -right,
(float *)result.getData(), getRows()*getCols()); (float *)result.getData(), getRows()*getCols());
return result; return result;
} }
inline Matrix operator*(float right) const inline Matrix operator*(float right) const {
{
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
arm_mat_scale_f32(&_matrix, right, arm_mat_scale_f32(&_matrix, right,
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline Matrix operator/(float right) const inline Matrix operator/(float right) const {
{
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
arm_mat_scale_f32(&_matrix, 1.0f / right, arm_mat_scale_f32(&_matrix, 1.0f / right,
&(result._matrix)); &(result._matrix));
return result; return result;
} }
// vector ops // vector ops
inline Vector operator*(const Vector & right) const inline Vector operator*(const Vector &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getCols() == right.getRows()); ASSERT(getCols() == right.getRows());
#endif #endif
@@ -190,8 +176,7 @@ public:
return Vector(getRows(), resultMat.getData()); return Vector(getRows(), resultMat.getData());
} }
// matrix ops // matrix ops
inline Matrix operator+(const Matrix & right) const inline Matrix operator+(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols() == right.getCols()); ASSERT(getCols() == right.getCols());
@@ -201,8 +186,7 @@ public:
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline Matrix operator-(const Matrix & right) const inline Matrix operator-(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols() == right.getCols()); ASSERT(getCols() == right.getCols());
@@ -212,8 +196,7 @@ public:
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline Matrix operator*(const Matrix & right) const inline Matrix operator*(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getCols() == right.getRows()); ASSERT(getCols() == right.getRows());
#endif #endif
@@ -222,8 +205,7 @@ public:
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline Matrix operator/(const Matrix & right) const inline Matrix operator/(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(right.getRows() == right.getCols()); ASSERT(right.getRows() == right.getCols());
ASSERT(getCols() == right.getCols()); ASSERT(getCols() == right.getCols());
@@ -231,24 +213,23 @@ public:
return (*this) * right.inverse(); return (*this) * right.inverse();
} }
// other functions // other functions
inline Matrix transpose() const inline Matrix transpose() const {
{
Matrix result(getCols(), getRows()); Matrix result(getCols(), getRows());
arm_mat_trans_f32(&_matrix, &(result._matrix)); arm_mat_trans_f32(&_matrix, &(result._matrix));
return result; return result;
} }
inline void swapRows(size_t a, size_t b) inline void swapRows(size_t a, size_t b) {
{
if (a == b) return; if (a == b) return;
for (size_t j = 0; j < getCols(); j++) { for (size_t j = 0; j < getCols(); j++) {
float tmp = (*this)(a, j); float tmp = (*this)(a, j);
(*this)(a, j) = (*this)(b, j); (*this)(a, j) = (*this)(b, j);
(*this)(b, j) = tmp; (*this)(b, j) = tmp;
} }
} }
inline void swapCols(size_t a, size_t b) inline void swapCols(size_t a, size_t b) {
{
if (a == b) return; if (a == b) return;
for (size_t i = 0; i < getRows(); i++) { for (size_t i = 0; i < getRows(); i++) {
float tmp = (*this)(i, a); float tmp = (*this)(i, a);
(*this)(i, a) = (*this)(i, b); (*this)(i, a) = (*this)(i, b);
@@ -258,8 +239,7 @@ public:
/** /**
* inverse based on LU factorization with partial pivotting * inverse based on LU factorization with partial pivotting
*/ */
Matrix inverse() const Matrix inverse() const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows() == getCols()); ASSERT(getRows() == getCols());
#endif #endif
@@ -269,25 +249,25 @@ public:
&(result._matrix)); &(result._matrix));
return result; return result;
} }
inline void setAll(const float & val) inline void setAll(const float &val) {
{
for (size_t i = 0; i < getRows(); i++) { for (size_t i = 0; i < getRows(); i++) {
for (size_t j = 0; j < getCols(); j++) { for (size_t j = 0; j < getCols(); j++) {
(*this)(i, j) = val; (*this)(i, j) = val;
} }
} }
} }
inline void set(const float * data) inline void set(const float *data) {
{
memcpy(getData(), data, getSize()); memcpy(getData(), data, getSize());
} }
inline size_t getRows() const { return _matrix.numRows; } inline size_t getRows() const { return _matrix.numRows; }
inline size_t getCols() const { return _matrix.numCols; } inline size_t getCols() const { return _matrix.numCols; }
inline static Matrix identity(size_t size) { inline static Matrix identity(size_t size) {
Matrix result(size, size); Matrix result(size, size);
for (size_t i = 0; i < size; i++) { for (size_t i = 0; i < size; i++) {
result(i, i) = 1.0f; result(i, i) = 1.0f;
} }
return result; return result;
} }
inline static Matrix zero(size_t size) { inline static Matrix zero(size_t size) {
apps/systemlib/math/arm/Vector.hpp
+31 -51
View File
@@ -55,111 +55,100 @@
namespace math namespace math
{ {
class __EXPORT Vector { class __EXPORT Vector
{
public: public:
// constructor // constructor
Vector(size_t rows) : Vector(size_t rows) :
_rows(rows), _rows(rows),
_data((float*)calloc(rows,sizeof(float))) _data((float *)calloc(rows, sizeof(float))) {
{
} }
Vector(size_t rows, const float *data) : Vector(size_t rows, const float *data) :
_rows(rows), _rows(rows),
_data((float*)malloc(getSize())) _data((float *)malloc(getSize())) {
{
memcpy(getData(), data, getSize()); memcpy(getData(), data, getSize());
} }
// deconstructor // deconstructor
virtual ~Vector() virtual ~Vector() {
{
delete [] getData(); delete [] getData();
} }
// copy constructor (deep) // copy constructor (deep)
Vector(const Vector &right) : Vector(const Vector &right) :
_rows(right.getRows()), _rows(right.getRows()),
_data((float*)malloc(getSize())) _data((float *)malloc(getSize())) {
{
memcpy(getData(), right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
// assignment // assignment
inline Vector & operator=(const Vector & right) inline Vector &operator=(const Vector &right) {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
if (this != &right)
{ if (this != &right) {
memcpy(getData(), right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
return *this; return *this;
} }
// element accessors // element accessors
inline float& operator()(size_t i) inline float &operator()(size_t i) {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(i < getRows()); ASSERT(i < getRows());
#endif #endif
return getData()[i]; return getData()[i];
} }
inline const float& operator()(size_t i) const inline const float &operator()(size_t i) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(i < getRows()); ASSERT(i < getRows());
#endif #endif
return getData()[i]; return getData()[i];
} }
// output // output
inline void print() const inline void print() const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++)
{
float sig; float sig;
int exp; int exp;
float num = (*this)(i); float num = (*this)(i);
float2SigExp(num, sig, exp); float2SigExp(num, sig, exp);
printf("%6.3fe%03.3d,", (double)sig, exp); printf("%6.3fe%03.3d,", (double)sig, exp);
} }
printf("\n"); printf("\n");
} }
// boolean ops // boolean ops
inline bool operator==(const Vector & right) const inline bool operator==(const Vector &right) const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++)
{
if (fabsf(((*this)(i) - right(i))) > 1e-30f) if (fabsf(((*this)(i) - right(i))) > 1e-30f)
return false; return false;
} }
return true; return true;
} }
// scalar ops // scalar ops
inline Vector operator+(float right) const inline Vector operator+(float right) const {
{
Vector result(getRows()); Vector result(getRows());
arm_offset_f32((float *)getData(), arm_offset_f32((float *)getData(),
right, result.getData(), right, result.getData(),
getRows()); getRows());
return result; return result;
} }
inline Vector operator-(float right) const inline Vector operator-(float right) const {
{
Vector result(getRows()); Vector result(getRows());
arm_offset_f32((float *)getData(), arm_offset_f32((float *)getData(),
-right, result.getData(), -right, result.getData(),
getRows()); getRows());
return result; return result;
} }
inline Vector operator*(float right) const inline Vector operator*(float right) const {
{
Vector result(getRows()); Vector result(getRows());
arm_scale_f32((float *)getData(), arm_scale_f32((float *)getData(),
right, result.getData(), right, result.getData(),
getRows()); getRows());
return result; return result;
} }
inline Vector operator/(float right) const inline Vector operator/(float right) const {
{
Vector result(getRows()); Vector result(getRows());
arm_scale_f32((float *)getData(), arm_scale_f32((float *)getData(),
1.0f / right, result.getData(), 1.0f / right, result.getData(),
@@ -167,8 +156,7 @@ public:
return result; return result;
} }
// vector ops // vector ops
inline Vector operator+(const Vector & right) const inline Vector operator+(const Vector &right) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
@@ -179,8 +167,7 @@ public:
getRows()); getRows());
return result; return result;
} }
inline Vector operator-(const Vector & right) const inline Vector operator-(const Vector &right) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
@@ -192,8 +179,7 @@ public:
return result; return result;
} }
// other functions // other functions
inline float dot(const Vector & right) inline float dot(const Vector &right) {
{
float result = 0; float result = 0;
arm_dot_prod_f32((float *)getData(), arm_dot_prod_f32((float *)getData(),
(float *)right.getData(), (float *)right.getData(),
@@ -201,29 +187,23 @@ public:
&result); &result);
return result; return result;
} }
inline float norm() inline float norm() {
{
return sqrtf(dot(*this)); return sqrtf(dot(*this));
} }
inline Vector unit() inline Vector unit() {
{
return (*this) / norm(); return (*this) / norm();
} }
inline static Vector zero(size_t rows) inline static Vector zero(size_t rows) {
{
Vector result(rows); Vector result(rows);
// calloc returns zeroed memory // calloc returns zeroed memory
return result; return result;
} }
inline void setAll(const float & val) inline void setAll(const float &val) {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0;i<getRows();i++)
{
(*this)(i) = val; (*this)(i) = val;
} }
} }
inline void set(const float * data) inline void set(const float *data) {
{
memcpy(getData(), data, getSize()); memcpy(getData(), data, getSize());
} }
inline size_t getRows() const { return _rows; } inline size_t getRows() const { return _rows; }
apps/systemlib/math/generic/Matrix.hpp
+85 -95
View File
@@ -53,61 +53,56 @@
namespace math namespace math
{ {
class __EXPORT Matrix { class __EXPORT Matrix
{
public: public:
// constructor // constructor
Matrix(size_t rows, size_t cols) : Matrix(size_t rows, size_t cols) :
_rows(rows), _rows(rows),
_cols(cols), _cols(cols),
_data((float*)calloc(rows*cols,sizeof(float))) _data((float *)calloc(rows *cols, sizeof(float))) {
{
} }
Matrix(size_t rows, size_t cols, const float *data) : Matrix(size_t rows, size_t cols, const float *data) :
_rows(rows), _rows(rows),
_cols(cols), _cols(cols),
_data((float*)malloc(getSize())) _data((float *)malloc(getSize())) {
{
memcpy(getData(), data, getSize()); memcpy(getData(), data, getSize());
} }
// deconstructor // deconstructor
virtual ~Matrix() virtual ~Matrix() {
{
delete [] getData(); delete [] getData();
} }
// copy constructor (deep) // copy constructor (deep)
Matrix(const Matrix &right) : Matrix(const Matrix &right) :
_rows(right.getRows()), _rows(right.getRows()),
_cols(right.getCols()), _cols(right.getCols()),
_data((float*)malloc(getSize())) _data((float *)malloc(getSize())) {
{
memcpy(getData(), right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
// assignment // assignment
inline Matrix & operator=(const Matrix & right) inline Matrix &operator=(const Matrix &right) {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols() == right.getCols()); ASSERT(getCols() == right.getCols());
#endif #endif
if (this != &right)
{ if (this != &right) {
memcpy(getData(), right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
return *this; return *this;
} }
// element accessors // element accessors
inline float & operator()(size_t i, size_t j) inline float &operator()(size_t i, size_t j) {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(i < getRows()); ASSERT(i < getRows());
ASSERT(j < getCols()); ASSERT(j < getCols());
#endif #endif
return getData()[i * getCols() + j]; return getData()[i * getCols() + j];
} }
inline const float & operator()(size_t i, size_t j) const inline const float &operator()(size_t i, size_t j) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(i < getRows()); ASSERT(i < getRows());
ASSERT(j < getCols()); ASSERT(j < getCols());
@@ -115,152 +110,138 @@ public:
return getData()[i * getCols() + j]; return getData()[i * getCols() + j];
} }
// output // output
inline void print() const inline void print() const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++) for (size_t j = 0; j < getCols(); j++) {
{
for (size_t j=0; j<getCols(); j++)
{
float sig; float sig;
int exp; int exp;
float num = (*this)(i, j); float num = (*this)(i, j);
float2SigExp(num, sig, exp); float2SigExp(num, sig, exp);
printf("%6.3fe%03.3d,", (double)sig, exp); printf("%6.3fe%03.3d,", (double)sig, exp);
} }
printf("\n"); printf("\n");
} }
} }
// boolean ops // boolean ops
inline bool operator==(const Matrix & right) const inline bool operator==(const Matrix &right) const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++) for (size_t j = 0; j < getCols(); j++) {
{
for (size_t j=0; j<getCols(); j++)
{
if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
return false; return false;
} }
} }
return true; return true;
} }
// scalar ops // scalar ops
inline Matrix operator+(const float & right) const inline Matrix operator+(const float &right) const {
{
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t j=0; j<getCols(); j++) for (size_t j = 0; j < getCols(); j++) {
{
result(i, j) = (*this)(i, j) + right; result(i, j) = (*this)(i, j) + right;
} }
} }
return result; return result;
} }
inline Matrix operator-(const float & right) const inline Matrix operator-(const float &right) const {
{
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t j=0; j<getCols(); j++) for (size_t j = 0; j < getCols(); j++) {
{
result(i, j) = (*this)(i, j) - right; result(i, j) = (*this)(i, j) - right;
} }
} }
return result; return result;
} }
inline Matrix operator*(const float & right) const inline Matrix operator*(const float &right) const {
{
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t j=0; j<getCols(); j++) for (size_t j = 0; j < getCols(); j++) {
{
result(i, j) = (*this)(i, j) * right; result(i, j) = (*this)(i, j) * right;
} }
} }
return result; return result;
} }
inline Matrix operator/(const float & right) const inline Matrix operator/(const float &right) const {
{
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t j=0; j<getCols(); j++) for (size_t j = 0; j < getCols(); j++) {
{
result(i, j) = (*this)(i, j) / right; result(i, j) = (*this)(i, j) / right;
} }
} }
return result; return result;
} }
// vector ops // vector ops
inline Vector operator*(const Vector & right) const inline Vector operator*(const Vector &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getCols() == right.getRows()); ASSERT(getCols() == right.getRows());
#endif #endif
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t j=0; j<getCols(); j++) for (size_t j = 0; j < getCols(); j++) {
{
result(i) += (*this)(i, j) * right(j); result(i) += (*this)(i, j) * right(j);
} }
} }
return result; return result;
} }
// matrix ops // matrix ops
inline Matrix operator+(const Matrix & right) const inline Matrix operator+(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols() == right.getCols()); ASSERT(getCols() == right.getCols());
#endif #endif
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t j=0; j<getCols(); j++) for (size_t j = 0; j < getCols(); j++) {
{
result(i, j) = (*this)(i, j) + right(i, j); result(i, j) = (*this)(i, j) + right(i, j);
} }
} }
return result; return result;
} }
inline Matrix operator-(const Matrix & right) const inline Matrix operator-(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
ASSERT(getCols() == right.getCols()); ASSERT(getCols() == right.getCols());
#endif #endif
Matrix result(getRows(), getCols()); Matrix result(getRows(), getCols());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t j=0; j<getCols(); j++) for (size_t j = 0; j < getCols(); j++) {
{
result(i, j) = (*this)(i, j) - right(i, j); result(i, j) = (*this)(i, j) - right(i, j);
} }
} }
return result; return result;
} }
inline Matrix operator*(const Matrix & right) const inline Matrix operator*(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getCols() == right.getRows()); ASSERT(getCols() == right.getRows());
#endif #endif
Matrix result(getRows(), right.getCols()); Matrix result(getRows(), right.getCols());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t j=0; j<right.getCols(); j++) for (size_t j = 0; j < right.getCols(); j++) {
{ for (size_t k = 0; k < right.getRows(); k++) {
for (size_t k=0; k<right.getRows(); k++)
{
result(i, j) += (*this)(i, k) * right(k, j); result(i, j) += (*this)(i, k) * right(k, j);
} }
} }
} }
return result; return result;
} }
inline Matrix operator/(const Matrix & right) const inline Matrix operator/(const Matrix &right) const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(right.getRows() == right.getCols()); ASSERT(right.getRows() == right.getCols());
ASSERT(getCols() == right.getCols()); ASSERT(getCols() == right.getCols());
@@ -268,28 +249,29 @@ public:
return (*this) * right.inverse(); return (*this) * right.inverse();
} }
// other functions // other functions
inline Matrix transpose() const inline Matrix transpose() const {
{
Matrix result(getCols(), getRows()); Matrix result(getCols(), getRows());
for (size_t i = 0; i < getRows(); i++) { for (size_t i = 0; i < getRows(); i++) {
for (size_t j = 0; j < getCols(); j++) { for (size_t j = 0; j < getCols(); j++) {
result(j, i) = (*this)(i, j); result(j, i) = (*this)(i, j);
} }
} }
return result; return result;
} }
inline void swapRows(size_t a, size_t b) inline void swapRows(size_t a, size_t b) {
{
if (a == b) return; if (a == b) return;
for (size_t j = 0; j < getCols(); j++) { for (size_t j = 0; j < getCols(); j++) {
float tmp = (*this)(a, j); float tmp = (*this)(a, j);
(*this)(a, j) = (*this)(b, j); (*this)(a, j) = (*this)(b, j);
(*this)(b, j) = tmp; (*this)(b, j) = tmp;
} }
} }
inline void swapCols(size_t a, size_t b) inline void swapCols(size_t a, size_t b) {
{
if (a == b) return; if (a == b) return;
for (size_t i = 0; i < getRows(); i++) { for (size_t i = 0; i < getRows(); i++) {
float tmp = (*this)(i, a); float tmp = (*this)(i, a);
(*this)(i, a) = (*this)(i, b); (*this)(i, a) = (*this)(i, b);
@@ -299,8 +281,7 @@ public:
/** /**
* inverse based on LU factorization with partial pivotting * inverse based on LU factorization with partial pivotting
*/ */
Matrix inverse() const Matrix inverse() const {
{
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
ASSERT(getRows() == getCols()); ASSERT(getRows() == getCols());
#endif #endif
@@ -320,6 +301,7 @@ public:
//printf("trying pivot for row %d\n",n); //printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i < N; i++) {
if (i == n) continue; if (i == n) continue;
//printf("\ttrying row %d\n",i); //printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) { if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i); //printf("swapped %d\n",i);
@@ -328,6 +310,7 @@ public:
} }
} }
} }
#ifdef MATRIX_ASSERT #ifdef MATRIX_ASSERT
//printf("A:\n"); A.print(); //printf("A:\n"); A.print();
//printf("U:\n"); U.print(); //printf("U:\n"); U.print();
@@ -335,15 +318,16 @@ public:
//fflush(stdout); //fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f); ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif #endif
// failsafe, return zero matrix // failsafe, return zero matrix
if (fabsf(U(n,n))<1e-8f) if (fabsf(U(n, n)) < 1e-8f) {
{
return Matrix::zero(n); return Matrix::zero(n);
} }
// for all rows below diagonal // for all rows below diagonal
for (size_t i = (n + 1); i < N; i++) { for (size_t i = (n + 1); i < N; i++) {
L(i, n) = U(i, n) / U(n, n); L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row // add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n) // multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < N; k++) { for (size_t k = n; k < N; k++) {
@@ -357,6 +341,7 @@ public:
// solve LY=P*I for Y by forward subst // solve LY=P*I for Y by forward subst
Matrix Y = P; Matrix Y = P;
// for all columns of Y // for all columns of Y
for (size_t c = 0; c < N; c++) { for (size_t c = 0; c < N; c++) {
// for all rows of L // for all rows of L
@@ -368,6 +353,7 @@ public:
// contribute to the solution // contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c); Y(i, c) -= L(i, j) * Y(j, c);
} }
// divide by the factor // divide by the factor
// on current // on current
// term to be solved // term to be solved
@@ -380,12 +366,14 @@ public:
// solve Ux=y for x by back subst // solve Ux=y for x by back subst
Matrix X = Y; Matrix X = Y;
// for all columns of X // for all columns of X
for (size_t c = 0; c < N; c++) { for (size_t c = 0; c < N; c++) {
// for all rows of U // for all rows of U
for (size_t k = 0; k < N; k++) { for (size_t k = 0; k < N; k++) {
// have to go in reverse order // have to go in reverse order
size_t i = N - 1 - k; size_t i = N - 1 - k;
// for all columns of U // for all columns of U
for (size_t j = i + 1; j < N; j++) { for (size_t j = i + 1; j < N; j++) {
// for all existing x // for all existing x
@@ -393,34 +381,36 @@ public:
// contribute to the solution // contribute to the solution
X(i, c) -= U(i, j) * X(j, c); X(i, c) -= U(i, j) * X(j, c);
} }
// divide by the factor // divide by the factor
// on current // on current
// term to be solved // term to be solved
X(i, c) /= U(i, i); X(i, c) /= U(i, i);
} }
} }
//printf("X:\n"); X.print(); //printf("X:\n"); X.print();
return X; return X;
} }
inline void setAll(const float & val) inline void setAll(const float &val) {
{
for (size_t i = 0; i < getRows(); i++) { for (size_t i = 0; i < getRows(); i++) {
for (size_t j = 0; j < getCols(); j++) { for (size_t j = 0; j < getCols(); j++) {
(*this)(i, j) = val; (*this)(i, j) = val;
} }
} }
} }
inline void set(const float * data) inline void set(const float *data) {
{
memcpy(getData(), data, getSize()); memcpy(getData(), data, getSize());
} }
inline size_t getRows() const { return _rows; } inline size_t getRows() const { return _rows; }
inline size_t getCols() const { return _cols; } inline size_t getCols() const { return _cols; }
inline static Matrix identity(size_t size) { inline static Matrix identity(size_t size) {
Matrix result(size, size); Matrix result(size, size);
for (size_t i = 0; i < size; i++) { for (size_t i = 0; i < size; i++) {
result(i, i) = 1.0f; result(i, i) = 1.0f;
} }
return result; return result;
} }
inline static Matrix zero(size_t size) { inline static Matrix zero(size_t size) {
apps/systemlib/math/generic/Vector.hpp
+52 -65
View File
@@ -51,179 +51,166 @@
namespace math namespace math
{ {
class __EXPORT Vector { class __EXPORT Vector
{
public: public:
// constructor // constructor
Vector(size_t rows) : Vector(size_t rows) :
_rows(rows), _rows(rows),
_data((float*)calloc(rows,sizeof(float))) _data((float *)calloc(rows, sizeof(float))) {
{
} }
Vector(size_t rows, const float *data) : Vector(size_t rows, const float *data) :
_rows(rows), _rows(rows),
_data((float*)malloc(getSize())) _data((float *)malloc(getSize())) {
{
memcpy(getData(), data, getSize()); memcpy(getData(), data, getSize());
} }
// deconstructor // deconstructor
virtual ~Vector() virtual ~Vector() {
{
delete [] getData(); delete [] getData();
} }
// copy constructor (deep) // copy constructor (deep)
Vector(const Vector &right) : Vector(const Vector &right) :
_rows(right.getRows()), _rows(right.getRows()),
_data((float*)malloc(getSize())) _data((float *)malloc(getSize())) {
{
memcpy(getData(), right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
// assignment // assignment
inline Vector & operator=(const Vector & right) inline Vector &operator=(const Vector &right) {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
if (this != &right)
{ if (this != &right) {
memcpy(getData(), right.getData(), memcpy(getData(), right.getData(),
right.getSize()); right.getSize());
} }
return *this; return *this;
} }
// element accessors // element accessors
inline float& operator()(size_t i) inline float &operator()(size_t i) {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(i < getRows()); ASSERT(i < getRows());
#endif #endif
return getData()[i]; return getData()[i];
} }
inline const float& operator()(size_t i) const inline const float &operator()(size_t i) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(i < getRows()); ASSERT(i < getRows());
#endif #endif
return getData()[i]; return getData()[i];
} }
// output // output
inline void print() const inline void print() const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++)
{
float sig; float sig;
int exp; int exp;
float num = (*this)(i); float num = (*this)(i);
float2SigExp(num, sig, exp); float2SigExp(num, sig, exp);
printf("%6.3fe%03.3d,", (double)sig, exp); printf("%6.3fe%03.3d,", (double)sig, exp);
} }
printf("\n"); printf("\n");
} }
// boolean ops // boolean ops
inline bool operator==(const Vector & right) const inline bool operator==(const Vector &right) const {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0; i<getRows(); i++)
{
if (fabsf(((*this)(i) - right(i))) > 1e-30f) if (fabsf(((*this)(i) - right(i))) > 1e-30f)
return false; return false;
} }
return true; return true;
} }
// scalar ops // scalar ops
inline Vector operator+(const float & right) const inline Vector operator+(const float &right) const {
{
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result(i) = (*this)(i) + right; result(i) = (*this)(i) + right;
} }
return result; return result;
} }
inline Vector operator-(const float & right) const inline Vector operator-(const float &right) const {
{
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result(i) = (*this)(i) - right; result(i) = (*this)(i) - right;
} }
return result; return result;
} }
inline Vector operator*(const float & right) const inline Vector operator*(const float &right) const {
{
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result(i) = (*this)(i) * right; result(i) = (*this)(i) * right;
} }
return result; return result;
} }
inline Vector operator/(const float & right) const inline Vector operator/(const float &right) const {
{
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result(i) = (*this)(i) / right; result(i) = (*this)(i) / right;
} }
return result; return result;
} }
// vector ops // vector ops
inline Vector operator+(const Vector & right) const inline Vector operator+(const Vector &right) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result(i) = (*this)(i) + right(i); result(i) = (*this)(i) + right(i);
} }
return result; return result;
} }
inline Vector operator-(const Vector & right) const inline Vector operator-(const Vector &right) const {
{
#ifdef VECTOR_ASSERT #ifdef VECTOR_ASSERT
ASSERT(getRows() == right.getRows()); ASSERT(getRows() == right.getRows());
#endif #endif
Vector result(getRows()); Vector result(getRows());
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result(i) = (*this)(i) - right(i); result(i) = (*this)(i) - right(i);
} }
return result; return result;
} }
// other functions // other functions
inline float dot(const Vector & right) inline float dot(const Vector &right) {
{
float result = 0; float result = 0;
for (size_t i=0; i<getRows(); i++)
{ for (size_t i = 0; i < getRows(); i++) {
result += (*this)(i) * (*this)(i); result += (*this)(i) * (*this)(i);
} }
return result; return result;
} }
inline float norm() inline float norm() {
{
return sqrtf(dot(*this)); return sqrtf(dot(*this));
} }
inline Vector unit() inline Vector unit() {
{
return (*this) / norm(); return (*this) / norm();
} }
inline static Vector zero(size_t rows) inline static Vector zero(size_t rows) {
{
Vector result(rows); Vector result(rows);
// calloc returns zeroed memory // calloc returns zeroed memory
return result; return result;
} }
inline void setAll(const float & val) inline void setAll(const float &val) {
{ for (size_t i = 0; i < getRows(); i++) {
for (size_t i=0;i<getRows();i++)
{
(*this)(i) = val; (*this)(i) = val;
} }
} }
inline void set(const float * data) inline void set(const float *data) {
{
memcpy(getData(), data, getSize()); memcpy(getData(), data, getSize());
} }
inline size_t getRows() const { return _rows; } inline size_t getRows() const { return _rows; }
apps/systemlib/test/test.cpp
+13 -8
View File
@@ -45,12 +45,12 @@
bool __EXPORT equal(float a, float b, float epsilon) bool __EXPORT equal(float a, float b, float epsilon)
{ {
float diff = fabsf(a - b); float diff = fabsf(a - b);
if (diff>epsilon)
{ if (diff > epsilon) {
printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false; return false;
}
else return true; } else return true;
} }
void __EXPORT float2SigExp( void __EXPORT float2SigExp(
@@ -58,28 +58,33 @@ void __EXPORT float2SigExp(
float &sig, float &sig,
int &exp) int &exp)
{ {
if (isnan(num) || isinf(num)) if (isnan(num) || isinf(num)) {
{
sig = 0.0f; sig = 0.0f;
exp = -99; exp = -99;
return; return;
} }
if (fabsf(num) < 1.0e-38f)
{ if (fabsf(num) < 1.0e-38f) {
sig = 0; sig = 0;
exp = 0; exp = 0;
return; return;
} }
exp = log10f(fabsf(num)); exp = log10f(fabsf(num));
if (exp > 0) { if (exp > 0) {
exp = ceil(exp); exp = ceil(exp);
} else { } else {
exp = floor(exp); exp = floor(exp);
} }
sig = num; sig = num;
// cheap power since it is integer // cheap power since it is integer
if (exp > 0) { if (exp > 0) {
for (int i = 0; i < abs(exp); i++) sig /= 10; for (int i = 0; i < abs(exp); i++) sig /= 10;
} else { } else {
for (int i = 0; i < abs(exp); i++) sig *= 10; for (int i = 0; i < abs(exp); i++) sig *= 10;
} }