aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 09:39: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) {};
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;
}

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) {};
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;

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) {};
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);
}

42
flix/vector.h
View File

@ -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);