FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

fixed some issues

Browse Source 
added new pose/turn detections
new helper classes
define-flags for libEigen
This commit is contained in:
FrankE
2018-09-04 10:49:00 +02:00
parent f990485d44
commit 857d7a1553
51 changed files with 2149 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
math/Matrix3.h
View File

@@ -4,6 +4,7 @@
#include <initializer_list>
#include <cmath>
#include "../Assertions.h"
class Matrix3 {
@@ -24,30 +25,72 @@ public:
return Matrix3( {1,0,0, 0,1,0, 0,0,1} );
}
// static Matrix3 getRotationDeg(const float degX, const float degY, const float degZ) {
// return getRotationRad(degX/180.0f*M_PI, degY/180.0f*M_PI, degZ/180.0f*M_PI);
// }
static Matrix3 getRotationDeg(const float degX, const float degY, const float degZ) {
return getRotationRad(degX/180.0f*M_PI, degY/180.0f*M_PI, degZ/180.0f*M_PI);
}
// static Matrix4 getRotationRad(const float radX, const float radY, const float radZ) {
static Matrix3 getRotationRad(const float radX, const float radY, const float radZ) {
// const float g = radX; const float b = radY; const float a = radZ;
// const float a11 = std::cos(a)*std::cos(b);
// const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
// const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
// const float a21 = std::sin(a)*std::cos(b);
// const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
// const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
// const float a31 = -std::sin(b);
// const float a32 = std::cos(b)*std::sin(g);
// const float a33 = std::cos(b)*std::cos(g);
// return Matrix4({
// a11, a12, a13, 0,
// a21, a22, a23, 0,
// a31, a32, a33, 0,
// 0, 0, 0, 1
// });
const float g = radX; const float b = radY; const float a = radZ;
const float a11 = std::cos(a)*std::cos(b);
const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
const float a21 = std::sin(a)*std::cos(b);
const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
const float a31 = -std::sin(b);
const float a32 = std::cos(b)*std::sin(g);
const float a33 = std::cos(b)*std::cos(g);
return Matrix3({
a11, a12, a13,
a21, a22, a23,
a31, a32, a33,
});
// }
}
static Matrix3 getRotationVec(const float nx, const float ny, const float nz, const float mag) {
Assert::isNotNaN(nx, "detected NaN");
Assert::isNotNaN(ny, "detected NaN");
Assert::isNotNaN(nz, "detected NaN");
Assert::isNotNaN(mag, "detected NaN");
const float c = std::cos(mag);
const float s = std::sin(mag);
return Matrix3({
c+nx*nx*(1-c), nx*ny*(1-c)+nz*s, nx*nz*(1-c)-ny*s,
ny*nx*(1-c)-nz*s, c+ny*ny*(1-c), ny*nz*(1-c)+nx*s,
nz*nx*(1-c)+ny*s, nz*ny*(1-c)-nx*s, c+nz*nz*(1-c)
});
}
static Matrix3 getRotationRadX(const float x) {
return Matrix3({
1, 0, 0,
0, cos(x), -sin(x),
0, sin(x), cos(x)
});
}
static Matrix3 getRotationRadY(const float y) {
return Matrix3({
cos(y), 0, sin(y),
0, 1, 0,
-sin(y),0, cos(y)
});
}
static Matrix3 getRotationRadZ(const float z) {
return Matrix3({
cos(z), -sin(z), 0,
sin(z), cos(z), 0,
0, 0, 1
});
}
Matrix3 transposed() const {
return Matrix3({
data[0], data[3], data[6],
data[1], data[4], data[7],
data[2], data[5], data[8]
});
}
static Matrix3 getTranslation(const float x, const float y) {
return Matrix3({
@@ -126,6 +169,27 @@ struct Vector3 {
return Vector3(x/v, y/v, z/v);
}
Vector3& operator += (const Vector3 o) {
this->x += o.x;
this->y += o.y;
this->z += o.z;
return *this;
}
Vector3& operator -= (const Vector3 o) {
this->x -= o.x;
this->y -= o.y;
this->z -= o.z;
return *this;
}
// Vector& operator = (const float val) {
// this->x = val;
// this->y = val;
// this->z = val;
// return *this;
// }
bool operator == (const Vector3 o) const {
return (x==o.x) && (y==o.y) && (z==o.z);
}