Определены и реализованы основные функции кватернионов

Source/INS/geometry/quaternion.c

@@ -1,4 +1,64 @@
 #include "quaternion.h"
 
+Quaternion QuatConjugate(const Quaternion* q)
+{
+  Quaternion res = {.x = -q->x, .y = -q->y, .z = -q->z, .w = q->w};
+  return res;
+}
 
+Quaternion QuatInvert(const Quaternion* q)
+{
+	Quaternion res;
+	float nsq = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
+	
+	if (nsq > 1e-6f)
+	{
+		nsq = 1.0f / nsq;
+		
+		res.x = q->x * nsq;
+		res.y = q->y * nsq;
+		res.z = q->z * nsq;
+		res.w = q->w * nsq;
+		
+		return res;
+	}
+	
+	return *q;
+}
 
+Quaternion QuatNegate(const Quaternion* q)
+{
+	Quaternion res = {.x = -q->x, .y = -q->y, .z = -q->z, .w = -q->w};
+	return res;
+}
+
+Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2)
+{
+	Quaternion res = {.x = q1->x + q2->x, .y = q1->y + q2->y, .z = q1->z + q2->z, .w = q1->w + q2->w};
+	return res;
+}
+
+Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2)
+{
+	Quaternion res = {.x = q1->x - q2->x, .y = q1->y - q2->y, .z = q1->z - q2->z, .w = q1->w - q2->w};
+	return res;
+}
+
+Quaternion QuatConstProd(const Quaternion* q, const float value)
+{
+	Quaternion res = {.x = q->x * value, .y = q->y * value, .z = q->z * value, .w = q->w * value};
+	return res;
+}
+
+Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
+{
+	Quaternion res = {
+	.x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
+	.y = q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
+	.z = q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
+	.w = q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
+	};
+	
+	
+	return res;
+}

Source/INS/geometry/quaternion.h

@@ -3,7 +3,27 @@
 #ifndef QUATERNION_H
 #define QUATERNION_H
 
+#include "vector.h"
+#include <stdbool.h>
 
+typedef struct
+{
+	float x, y, z, w;
+} Quaternion;
+
+Quaternion QuatNormalize(const Quaternion* q, const float gain);
+Quaternion QuatConjugate(const Quaternion* q);
+Quaternion QuatInvert(const Quaternion* q);
+Quaternion QuatNegate(const Quaternion* q);
+
+Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2);
+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);
+Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad);
+Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay);
 
 
 #endif