Логика обработки данных иму реализована как в рабочей прошивке. Дополнены функции вектора

Source/INS/geometry/vector.c

@@ -10,8 +10,29 @@ Vector2 normalizeV2(const Vector2* v, float gain)
 
 Vector3 normalizeV3(const Vector3* v, float gain)
 {
-	float len = lengthV3(v);
-	Vector3 res = {.x = v->x / len, .y = v->y / len, .z = v->z};
+	Vector3 res = {0};
+	float n = lengthV3(v);
+	
+	if (n > 1e-12f)
+	{
+		n = gain / n;
+		res.x = v->x * n;
+		res.y = v->y * n;
+		res.z = v->z * n;
+	}
+	
+	return res;
+}
+
+float DotV2(const Vector2* v1, const Vector2* v2)
+{
+	float res = v1->x * v2->x + v1->y * v2->y;
+	return res;
+}
+
+float DotV3(const Vector3* v1, const Vector3* v2)
+{
+	float res = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
 	return res;
 }
 
@@ -129,6 +150,21 @@ float scalarProdV3(const Vector3* v1, const Vector3* v2)
 }
 
 
+Vector3 Cross(const Vector3* v1, const Vector3* v2)
+{
+	Vector3 res = 
+	{
+		v1->x * v2->z - v1->z * v2->y,
+		v1->z * v2->x - v1->x * v2->z,
+		v1->x * v2->y - v1->y * v2->x
+	};
+	
+	return res;
+}
+
+
+
+