From 410fccf0154ffbf7e874cf63297d2eff215d2cbc Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 6 Feb 2024 13:50:56 +0300 Subject: [PATCH] Fix vector, quaternion, pid and lpf libraries curly braces code style --- flix/lpf.h | 9 +++------ flix/pid.h | 6 ++---- flix/quaternion.h | 51 ++++++++++++++++------------------------------- flix/vector.h | 42 +++++++++++++------------------------- 4 files changed, 36 insertions(+), 72 deletions(-) diff --git a/flix/lpf.h b/flix/lpf.h index 937078f..56c231e 100644 --- a/flix/lpf.h +++ b/flix/lpf.h @@ -13,8 +13,7 @@ public: LowPassFilter(float alpha): alpha(alpha) {}; - T update(const T input) - { + T update(const T input) { if (alpha == 1) { // filter disabled return input; } @@ -26,13 +25,11 @@ public: 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); } - void reset() - { + void reset() { initialized = false; } diff --git a/flix/pid.h b/flix/pid.h index a330349..cdfbcae 100644 --- a/flix/pid.h +++ b/flix/pid.h @@ -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) {}; - float update(float error, float dt) - { + float update(float error, float dt) { integral += error * dt; if (isfinite(prevError) && dt > 0) { @@ -38,8 +37,7 @@ public: return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID } - void reset() - { + void reset() { prevError = NAN; integral = 0; derivative = 0; diff --git a/flix/quaternion.h b/flix/quaternion.h index 115eeab..8aee072 100644 --- a/flix/quaternion.h +++ b/flix/quaternion.h @@ -15,8 +15,7 @@ public: 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 sin2 = sin(halfAngle); float cos2 = cos(halfAngle); @@ -24,16 +23,14 @@ public: return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm); } - static Quaternion fromAngularRates(const Vector& rates) - { + static Quaternion fromAngularRates(const Vector& rates) { if (rates.zero()) { return Quaternion(); } 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 cy = cos(y / 2); float cz = cos(z / 2); @@ -48,8 +45,7 @@ public: 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 w1 = u.y * v.z - u.z * v.y; float w2 = u.z * v.x - u.x * v.z; @@ -64,24 +60,21 @@ public: 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; a = x / sin(angle / 2); b = y / sin(angle / 2); c = z / sin(angle / 2); } - Vector toEulerZYX() const - { + Vector toEulerZYX() const { return Vector( atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)), asin(2 * (w * y - z * x)), 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 float yaw; float sqx = x * x; @@ -101,15 +94,13 @@ public: return yaw; } - void setYaw(float yaw) - { + void setYaw(float yaw) { // TODO: optimize? Vector euler = toEulerZYX(); (*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw); } - Quaternion& operator *= (const Quaternion& q) - { + Quaternion& operator *= (const Quaternion& q) { Quaternion ret( w * q.w - x * q.x - y * q.y - z * q.z, w * q.x + x * q.w + y * q.z - z * q.y, @@ -118,8 +109,7 @@ public: return (*this = ret); } - Quaternion operator * (const Quaternion& q) - { + Quaternion operator * (const Quaternion& q) { return Quaternion( w * q.w - x * q.x - y * q.y - z * q.z, 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); } - Quaternion inversed() const - { + Quaternion inversed() const { float normSqInv = 1 / (w * w + x * x + y * y + z * z); return Quaternion( w * normSqInv, @@ -137,13 +126,11 @@ public: -z * normSqInv); } - float norm() const - { + float norm() const { return sqrt(w * w + x * x + y * y + z * z); } - void normalize() - { + void normalize() { float n = norm(); w /= n; x /= n; @@ -151,27 +138,23 @@ public: z /= n; } - Vector conjugate(const Vector& v) - { + Vector conjugate(const Vector& v) { Quaternion qv(0, v.x, v.y, v.z); Quaternion res = (*this) * qv * inversed(); 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 res = inversed() * qv * (*this); return Vector(res.x, res.y, res.z); } - inline Vector rotate(const Vector& v) - { + inline Vector rotate(const Vector& v) { return conjugateInversed(v); } - inline bool finite() const - { + inline bool finite() const { return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z); } diff --git a/flix/vector.h b/flix/vector.h index 5ed57a5..2f25e42 100644 --- a/flix/vector.h +++ b/flix/vector.h @@ -13,76 +13,62 @@ public: 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); } - bool zero() const - { + bool zero() const { return x == 0 && y == 0 && z == 0; } - void normalize() - { + void normalize() { float n = norm(); x /= n; y /= n; z /= n; } - Vector operator * (const float b) const - { + Vector operator * (const float b) const { 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); } - Vector operator + (const Vector& b) const - { + Vector operator + (const Vector& b) const { 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); } - inline bool operator == (const Vector& b) const - { + inline bool operator == (const Vector& b) const { 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); } - inline bool finite() const - { + inline bool finite() const { 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; } - 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); } - 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)); } - static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v) - { + static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v) { Vector direction = cross(u, v); direction.normalize(); float angle = angleBetweenVectors(u, v);