From 059b356ec772dc40751cb25b5484b73fa300673d Mon Sep 17 00:00:00 2001 From: dario Date: Wed, 19 Jun 2024 15:08:02 +0200 Subject: [PATCH] Added slerp and updated quaternion class --- .../math/algorithms/attitude/integrate.hpp | 1 + .../sta/math/algorithms/attitude/slerp.hpp | 23 +++++++++ include/sta/math/quaternion.hpp | 2 + include/sta/math/utils.hpp | 6 ++- src/algorithms/attitude/integrate.cpp | 4 ++ src/algorithms/attitude/slerp.cpp | 36 ++++++++++++++ src/quaternion.cpp | 24 +++++++++- src/utils.cpp | 47 ++++++++++++------- 8 files changed, 122 insertions(+), 21 deletions(-) create mode 100644 include/sta/math/algorithms/attitude/slerp.hpp create mode 100644 src/algorithms/attitude/slerp.cpp diff --git a/include/sta/math/algorithms/attitude/integrate.hpp b/include/sta/math/algorithms/attitude/integrate.hpp index 5d44585..eab80dc 100644 --- a/include/sta/math/algorithms/attitude/integrate.hpp +++ b/include/sta/math/algorithms/attitude/integrate.hpp @@ -28,6 +28,7 @@ namespace sta Quaternion getAttitude(); private: Quaternion state_; + float ox_, oy_, oz_; float alpha_; float time_; }; diff --git a/include/sta/math/algorithms/attitude/slerp.hpp b/include/sta/math/algorithms/attitude/slerp.hpp new file mode 100644 index 0000000..96b2fe1 --- /dev/null +++ b/include/sta/math/algorithms/attitude/slerp.hpp @@ -0,0 +1,23 @@ +/* + * slerp.hpp + * + * Created on: Jun 19, 2024 + * Author: Dario + */ + +#ifndef STA_MATHS_ATTITUDE_SLERP_HPP +#define STA_MATHS_ATTITUDE_SLERP_HPP + +#include + +namespace sta +{ + namespace math + { + + Quaternion slerp(Quaternion q1, Quaternion q2, float beta); + + } // namespace math +} // namespace sta + +#endif /* STA_MATHS_ATTITUDE_SLERP_HPP*/ diff --git a/include/sta/math/quaternion.hpp b/include/sta/math/quaternion.hpp index f937ff4..181e823 100644 --- a/include/sta/math/quaternion.hpp +++ b/include/sta/math/quaternion.hpp @@ -18,6 +18,8 @@ namespace sta Quaternion(); + Quaternion integrate(float dt, float ox, float oy, float oz); + static Quaternion unit(); float norm(); diff --git a/include/sta/math/utils.hpp b/include/sta/math/utils.hpp index 17efdd5..f3ee690 100644 --- a/include/sta/math/utils.hpp +++ b/include/sta/math/utils.hpp @@ -1,12 +1,16 @@ #ifndef INC_UTILS_HPP_ #define INC_UTILS_HPP_ +#include + namespace sta { namespace math { float fast_inv_sqrt(float); + + float clip(float value, float min, float max); } // namespace math } // namespace sta -#endif /* INC_UTILS_HPP_ */ \ No newline at end of file +#endif /* INC_UTILS_HPP_ */ diff --git a/src/algorithms/attitude/integrate.cpp b/src/algorithms/attitude/integrate.cpp index d98fd6b..20da6e1 100644 --- a/src/algorithms/attitude/integrate.cpp +++ b/src/algorithms/attitude/integrate.cpp @@ -6,6 +6,7 @@ */ #include +#include #include #include @@ -16,6 +17,9 @@ namespace sta { AttitudeModel::AttitudeModel(Quaternion state, float alpha /* = 1.0f */) : state_{state}, + ox_{0.0}, + oy_{0.0}, + oz_{0.0}, alpha_{alpha}, time_{sta::timeUs() / 1000000.0f} { diff --git a/src/algorithms/attitude/slerp.cpp b/src/algorithms/attitude/slerp.cpp new file mode 100644 index 0000000..f900a03 --- /dev/null +++ b/src/algorithms/attitude/slerp.cpp @@ -0,0 +1,36 @@ +/* + * slerp.cpp + * + * Created on: Jun 19, 2024 + * Author: Dario + */ + +#include +#include + +namespace sta +{ + namespace math + { + Quaternion slerp(Quaternion q1, Quaternion q2, float alpha) + { + float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; + + if (dot < 0.0) + { + q1 = q1 * -1; + dot = -1; + } + + float theta = std::acos(clip(dot, -1, 1)); + + if (theta < 0.00001) + { + return q1; + } + + return (q1 * std::sin((1-alpha) * theta) + q2 * std::sin(alpha * theta)) * (1 / std::sin(theta)); + } + } // namespace math +} // namespace sta + diff --git a/src/quaternion.cpp b/src/quaternion.cpp index cdee3b4..48f00cc 100644 --- a/src/quaternion.cpp +++ b/src/quaternion.cpp @@ -25,6 +25,26 @@ namespace sta } + Quaternion Quaternion::integrate(float dt, float ox, float oy, float oz) + { + ox *= (M_PI / 180.0f); + oy *= (M_PI / 180.0f); + oz *= (M_PI / 180.0f); + + float norm = sqrt(fmax(ox*ox + oy*oy + oz*oz, 0.000001)); + float dt2 = dt / 2; + + float cosDt2 = cos(norm * dt2); + float sinDt2 = 1/norm * sin(norm * dt2); + + float qw = cosDt2*w + -sinDt2*ox*x + -sinDt2*oy*y + -sinDt2*oz*z; + float qx = sinDt2*ox*w + cosDt2*x + sinDt2*oz*y + -sinDt2*oy*z; + float qy = sinDt2*oy*w + -sinDt2*oz*x + cosDt2*y + sinDt2*ox*z; + float qz = sinDt2*oz*w + sinDt2*oy*x + -sinDt2*ox*y + cosDt2*z; + + return Quaternion(qw, qx, qy, qz); + } + Quaternion Quaternion::unit() { return Quaternion(); @@ -44,12 +64,12 @@ namespace sta Quaternion Quaternion::operator+(const Quaternion& quat) { - return Quaternion(x + quat.x, y + quat.y, z + quat.z, w + quat.w); + return Quaternion(w + quat.w, x + quat.x, y + quat.y, z + quat.z); } Quaternion Quaternion::operator*(float scalar) { - return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar); + return Quaternion(w * scalar, x * scalar, y * scalar, z * scalar); } } // namespace math } // namespace sta diff --git a/src/utils.cpp b/src/utils.cpp index dba37c2..5d7fa43 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -3,29 +3,40 @@ namespace sta { + namespace math + { + float fast_inv_sqrt(float v) { -namespace math -{ + long i; + float x2, y; + const float threehalfs = 1.5f; - -float fast_inv_sqrt(float v) { + y = v; + x2 = y * 0.5f; + i = * (long*)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *) &i; + y = y * (threehalfs - (x2 * y * y)); + //y = y * (threehalfs - (x2 * y * y)); - long i; - float x2, y; - const float threehalfs = 1.5f; + return y; + } - y = v; - x2 = y * 0.5f; - i = * (long*)&y; - i = 0x5f3759df - (i >> 1); - y = *(float *) &i; - y = y * (threehalfs - (x2 * y * y)); - //y = y * (threehalfs - (x2 * y * y)); + float clip(float value, float min, float max) + { + if (value < min) + { + return min; + } - return y; + if (value > max) + { + return max; + } -} - -} // namespace math + return value; + } + + } // namespace math } // namespace sta