aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix vector, quaternion, pid and lpf libraries curly braces code style

Browse Source
This commit is contained in:
Oleg Kalachev 2024-02-06 13:50:56 +03:00
parent 31d382dd86
commit 410fccf015
4 changed files with 36 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flix/lpf.h
View File

@ -13,8 +13,7 @@ public:
LowPassFilter(float alpha): alpha(alpha) {}; LowPassFilter(float alpha): alpha(alpha) {};
T update(const T input) T update(const T input) {
{
if (alpha == 1) { // filter disabled if (alpha == 1) { // filter disabled
return input; return input;
} }
@ -26,13 +25,11 @@ public:
return output = output * (1 - alpha) + input * alpha; return output = output * (1 - alpha) + input * alpha;
} }
void setCutOffFrequency(float cutOffFreq, float dt) void setCutOffFrequency(float cutOffFreq, float dt) {
{
alpha = 1 - exp(-2 * PI * cutOffFreq * dt); alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
} }
void reset() void reset() {
{
initialized = false; initialized = false;
} }

6
flix/pid.h
View File

@ -21,8 +21,7 @@ public:
PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {}; PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {};
float update(float error, float dt) float update(float error, float dt) {
{
integral += error * dt; integral += error * dt;
if (isfinite(prevError) && dt > 0) { if (isfinite(prevError) && dt > 0) {
@ -38,8 +37,7 @@ public:
return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID
} }
void reset() void reset() {
{
prevError = NAN; prevError = NAN;
integral = 0; integral = 0;
derivative = 0; derivative = 0;

51
flix/quaternion.h
View File

@ -15,8 +15,7 @@ public:
Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {}; Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {};
static Quaternion fromAxisAngle(float a, float b, float c, float angle) static Quaternion fromAxisAngle(float a, float b, float c, float angle) {
{
float halfAngle = angle * 0.5; float halfAngle = angle * 0.5;
float sin2 = sin(halfAngle); float sin2 = sin(halfAngle);
float cos2 = cos(halfAngle); float cos2 = cos(halfAngle);
@ -24,16 +23,14 @@ public:
return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm); return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm);
} }
static Quaternion fromAngularRates(const Vector& rates) static Quaternion fromAngularRates(const Vector& rates) {
{
if (rates.zero()) { if (rates.zero()) {
return Quaternion(); return Quaternion();
} }
return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm()); return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());
} }
static Quaternion fromEulerZYX(float x, float y, float z) static Quaternion fromEulerZYX(float x, float y, float z) {
{
float cx = cos(x / 2); float cx = cos(x / 2);
float cy = cos(y / 2); float cy = cos(y / 2);
float cz = cos(z / 2); float cz = cos(z / 2);
@ -48,8 +45,7 @@ public:
cx * cy * sz - sx * sy * cz); cx * cy * sz - sx * sy * cz);
} }
static Quaternion fromBetweenVectors(Vector u, Vector v) static Quaternion fromBetweenVectors(Vector u, Vector v) {
{
float dot = u.x * v.x + u.y * v.y + u.z * v.z; float dot = u.x * v.x + u.y * v.y + u.z * v.z;
float w1 = u.y * v.z - u.z * v.y; float w1 = u.y * v.z - u.z * v.y;
float w2 = u.z * v.x - u.x * v.z; float w2 = u.z * v.x - u.x * v.z;
@ -64,24 +60,21 @@ public:
return ret; return ret;
} }
void toAxisAngle(float& a, float& b, float& c, float& angle) void toAxisAngle(float& a, float& b, float& c, float& angle) {
{
angle = acos(w) * 2; angle = acos(w) * 2;
a = x / sin(angle / 2); a = x / sin(angle / 2);
b = y / sin(angle / 2); b = y / sin(angle / 2);
c = z / sin(angle / 2); c = z / sin(angle / 2);
} }
Vector toEulerZYX() const Vector toEulerZYX() const {
{
return Vector( return Vector(
atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)), atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),
asin(2 * (w * y - z * x)), asin(2 * (w * y - z * x)),
atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))); atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)));
} }
float getYaw() const float getYaw() const {
{
// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122 // https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122
float yaw; float yaw;
float sqx = x * x; float sqx = x * x;
@ -101,15 +94,13 @@ public:
return yaw; return yaw;
} }
void setYaw(float yaw) void setYaw(float yaw) {
{
// TODO: optimize? // TODO: optimize?
Vector euler = toEulerZYX(); Vector euler = toEulerZYX();
(*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw); (*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw);
} }
Quaternion& operator *= (const Quaternion& q) Quaternion& operator *= (const Quaternion& q) {
{
Quaternion ret( Quaternion ret(
w * q.w - x * q.x - y * q.y - z * q.z, w * q.w - x * q.x - y * q.y - z * q.z,
w * q.x + x * q.w + y * q.z - z * q.y, w * q.x + x * q.w + y * q.z - z * q.y,
@ -118,8 +109,7 @@ public:
return (*this = ret); return (*this = ret);
} }
Quaternion operator * (const Quaternion& q) Quaternion operator * (const Quaternion& q) {
{
return Quaternion( return Quaternion(
w * q.w - x * q.x - y * q.y - z * q.z, w * q.w - x * q.x - y * q.y - z * q.z,
w * q.x + x * q.w + y * q.z - z * q.y, w * q.x + x * q.w + y * q.z - z * q.y,
@ -127,8 +117,7 @@ public:
w * q.z + z * q.w + x * q.y - y * q.x); w * q.z + z * q.w + x * q.y - y * q.x);
} }
Quaternion inversed() const Quaternion inversed() const {
{
float normSqInv = 1 / (w * w + x * x + y * y + z * z); float normSqInv = 1 / (w * w + x * x + y * y + z * z);
return Quaternion( return Quaternion(
w * normSqInv, w * normSqInv,
@ -137,13 +126,11 @@ public:
-z * normSqInv); -z * normSqInv);
} }
float norm() const float norm() const {
{
return sqrt(w * w + x * x + y * y + z * z); return sqrt(w * w + x * x + y * y + z * z);
} }
void normalize() void normalize() {
{
float n = norm(); float n = norm();
w /= n; w /= n;
x /= n; x /= n;
@ -151,27 +138,23 @@ public:
z /= n; z /= n;
} }
Vector conjugate(const Vector& v) Vector conjugate(const Vector& v) {
{
Quaternion qv(0, v.x, v.y, v.z); Quaternion qv(0, v.x, v.y, v.z);
Quaternion res = (*this) * qv * inversed(); Quaternion res = (*this) * qv * inversed();
return Vector(res.x, res.y, res.z); return Vector(res.x, res.y, res.z);
} }
Vector conjugateInversed(const Vector& v) Vector conjugateInversed(const Vector& v) {
{
Quaternion qv(0, v.x, v.y, v.z); Quaternion qv(0, v.x, v.y, v.z);
Quaternion res = inversed() * qv * (*this); Quaternion res = inversed() * qv * (*this);
return Vector(res.x, res.y, res.z); return Vector(res.x, res.y, res.z);
} }
inline Vector rotate(const Vector& v) inline Vector rotate(const Vector& v) {
{
return conjugateInversed(v); return conjugateInversed(v);
} }
inline bool finite() const inline bool finite() const {
{
return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z); return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
} }

42
flix/vector.h
View File

@ -13,76 +13,62 @@ public:
Vector(float x, float y, float z): x(x), y(y), z(z) {}; Vector(float x, float y, float z): x(x), y(y), z(z) {};
float norm() const float norm() const {
{
return sqrt(x * x + y * y + z * z); return sqrt(x * x + y * y + z * z);
} }
bool zero() const bool zero() const {
{
return x == 0 && y == 0 && z == 0; return x == 0 && y == 0 && z == 0;
} }
void normalize() void normalize() {
{
float n = norm(); float n = norm();
x /= n; x /= n;
y /= n; y /= n;
z /= n; z /= n;
} }
Vector operator * (const float b) const Vector operator * (const float b) const {
{
return Vector(x * b, y * b, z * b); return Vector(x * b, y * b, z * b);
} }
Vector operator / (const float b) const Vector operator / (const float b) const {
{
return Vector(x / b, y / b, z / b); return Vector(x / b, y / b, z / b);
} }
Vector operator + (const Vector& b) const Vector operator + (const Vector& b) const {
{
return Vector(x + b.x, y + b.y, z + b.z); return Vector(x + b.x, y + b.y, z + b.z);
} }
Vector operator - (const Vector& b) const Vector operator - (const Vector& b) const {
{
return Vector(x - b.x, y - b.y, z - b.z); return Vector(x - b.x, y - b.y, z - b.z);
} }
inline bool operator == (const Vector& b) const inline bool operator == (const Vector& b) const {
{
return x == b.x && y == b.y && z == b.z; return x == b.x && y == b.y && z == b.z;
} }
inline bool operator != (const Vector& b) const inline bool operator != (const Vector& b) const {
{
return !(*this == b); return !(*this == b);
} }
inline bool finite() const inline bool finite() const {
{
return isfinite(x) && isfinite(y) && isfinite(z); return isfinite(x) && isfinite(y) && isfinite(z);
} }
static float dot(const Vector& a, const Vector& b) static float dot(const Vector& a, const Vector& b) {
{
return a.x * b.x + a.y * b.y + a.z * b.z; return a.x * b.x + a.y * b.y + a.z * b.z;
} }
static Vector cross(const Vector& a, const Vector& b) static Vector cross(const Vector& a, const Vector& b) {
{
return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
} }
static float angleBetweenVectors(const Vector& a, const Vector& b) static float angleBetweenVectors(const Vector& a, const Vector& b) {
{
return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1)); return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1));
} }
static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v) static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v) {
{
Vector direction = cross(u, v); Vector direction = cross(u, v);
direction.normalize(); direction.normalize();
float angle = angleBetweenVectors(u, v); float angle = angleBetweenVectors(u, v);