Added slerp and updated quaternion class

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_;
 	    };

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 <sta/math/quaternion.hpp>
+
+namespace sta
+{
+	namespace math
+	{
+
+		Quaternion slerp(Quaternion q1, Quaternion q2, float beta);
+
+	}  // namespace math
+}  // namespace sta
+
+#endif /* STA_MATHS_ATTITUDE_SLERP_HPP*/

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

include/sta/math/utils.hpp

@@ -1,12 +1,16 @@
 #ifndef INC_UTILS_HPP_
 #define INC_UTILS_HPP_
 
+#include <math.h>
+
 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_ */
+#endif /* INC_UTILS_HPP_ */

src/algorithms/attitude/integrate.cpp

@@ -6,6 +6,7 @@
  */
 
 #include <sta/math/algorithms/attitude/integrate.hpp>
+#include <sta/math/algorithms/attitude/slerp.hpp>
 #include <sta/math/linalg/matrix.hpp>
 
 #include <math.h>
@@ -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}
 		{

src/algorithms/attitude/slerp.cpp

@@ -0,0 +1,36 @@
+/*
+ * slerp.cpp
+ *
+ *  Created on: Jun 19, 2024
+ *      Author: Dario
+ */
+
+#include <sta/math/algorithms/attitude/slerp.hpp>
+#include <sta/math/utils.hpp>
+
+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
+

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

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;
 
-        
+			y = v;
-float fast_inv_sqrt(float 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;
+			return y;
-    float x2, y;
+		}
-    const float threehalfs = 1.5f;
 
-    y = v;
+		float clip(float value, float min, float max)
-    x2 = y * 0.5f;
+		{
-    i = * (long*)&y;
+			if (value < min)
-    i = 0x5f3759df - (i >> 1);
+			{
-    y = *(float *) &i;
+				return  min;
-    y = y * (threehalfs - (x2 * y * y));
+			}
-    //y = y * (threehalfs - (x2 * y * y));
 
-    return y;
+			if (value > max)
+			{
+				return max;
+			}
 
-}
+			return value;
-    
+		}
-} // namespace math
+
+	} // namespace math
 
 } // namespace sta