Определён базовый функционал IRS

Source/INS/geometry/quaternion.c

@@ -1,4 +1,26 @@
 #include "quaternion.h"
+#include <math.h>
+
+Quaternion QuatNormalize(const Quaternion* q, const float gain)
+{
+	Quaternion res;
+	
+	float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
+	
+	if (norm > 1e-6f)
+	{
+		norm = gain / norm;
+		
+		res.x = q->x * norm;
+		res.y = q->y * norm;
+		res.z = q->z * norm;
+		res.w = q->w * norm;
+		
+		return res;
+	}
+	
+	return res;
+}
 
 Quaternion QuatConjugate(const Quaternion* q)
 {
@@ -62,3 +84,66 @@ Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
 	
 	return res;
 }
+
+Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW)
+{
+	Quaternion v = {vec->x, vec->y, 0, 0};
+	Quaternion h = {0, 0, CCW ? q->z : -q->z, q->w};
+	h = QuatNormalize(&h, 1.0f);
+
+	Quaternion vhprod = QuatProd(&v, &h);
+	h = QuatProd(&vhprod, &h);
+	
+	Vector3 res = {h.x, h.y, vec->z};
+	
+	return res;
+}
+
+Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad)
+{
+	Quaternion res;
+	
+	float h_r = 0.5f * RollPitchYawRad->y;
+	float h_p = 0.5f * RollPitchYawRad->x;
+	float h_y = 0.5f * RollPitchYawRad->z;
+	
+	float c_r = cosf(h_r), s_r = sinf(h_r);
+	float c_p = cosf(h_p), s_p = sinf(h_p);
+	float c_y = cosf(h_y), s_y = sinf(h_y);
+	
+	res.x = c_r * s_p * c_y - s_r * c_p * s_y;	// Был +, стал -
+	res.y = s_r * c_p * c_y + c_r * s_p * s_y;	// Был -, стал +
+	res.z = -c_r * c_p * s_y - s_r * s_p * c_y;	// Первое слагаемое стало отрицательным
+	res.w = c_r * c_p * c_y - s_r * s_p * s_y;	// Был +, стал -
+	
+	return res;
+}
+
+Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay)
+{
+  	Quaternion error = {
+	current->w * target->x + current->x * target->w + current->y * target->z - current->z * target->y,
+	current->w * target->y + current->x * target->z + current->y * target->w - current->z * target->x,
+	current->w * target->z + current->x * target->y + current->y * target->x - current->z * target->w,
+	current->w * target->w + current->x * target->x + current->y * target->y - current->z * target->z
+	};
+	
+	if (fastWay && error.w < 0.0f) return QuatNegate(&error);
+	
+	return error;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

Source/INS/geometry/quaternion.h

@@ -21,7 +21,7 @@ Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2);
 Quaternion QuatConstProd(const Quaternion* q, const float value);
 Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2);
 
-Vector3 QuatRotateAroundZ(const Vector3* vec, bool CCW);
+Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW);
 Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad);
 Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay);