Добавление сферической интерполяции, переход от применения acos к применению atan2, исправление ошибок

basic-geometry/slerp.h

@@ -0,0 +1,94 @@
+#ifndef _BGC_VERSOR_SLERP_H_
+#define _BGC_VERSOR_SLERP_H_
+
+#include "./versor.h"
+
+typedef struct {
+    float s0_cos_weight, s0_sin_weight;
+    float x1_cos_weight, x1_sin_weight;
+    float x2_cos_weight, x2_sin_weight;
+    float x3_cos_weight, x3_sin_weight;
+    float radians;
+} BgcSlerpFP32;
+
+typedef struct {
+    double s0_cos_weight, s0_sin_weight;
+    double x1_cos_weight, x1_sin_weight;
+    double x2_cos_weight, x2_sin_weight;
+    double x3_cos_weight, x3_sin_weight;
+    double radians;
+} BgcSlerpFP64;
+
+inline void bgc_slerp_reset_fp32(BgcSlerpFP32* slerp)
+{
+    slerp->s0_cos_weight = 1.0f;
+    slerp->s0_sin_weight = 0.0f;
+
+    slerp->x1_cos_weight = 0.0f;
+    slerp->x1_sin_weight = 0.0f;
+
+    slerp->x2_cos_weight = 0.0f;
+    slerp->x2_sin_weight = 0.0f;
+
+    slerp->x3_cos_weight = 0.0f;
+    slerp->x3_sin_weight = 0.0f;
+
+    slerp->radians = 0.0f;
+}
+
+inline void bgc_slerp_reset_fp64(BgcSlerpFP64* slerp)
+{
+    slerp->s0_cos_weight = 1.0;
+    slerp->s0_sin_weight = 0.0;
+
+    slerp->x1_cos_weight = 0.0;
+    slerp->x1_sin_weight = 0.0;
+
+    slerp->x2_cos_weight = 0.0;
+    slerp->x2_sin_weight = 0.0;
+
+    slerp->x3_cos_weight = 0.0;
+    slerp->x3_sin_weight = 0.0;
+
+    slerp->radians = 0.0;
+}
+
+void bgc_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp);
+
+void bgc_slerp_make_full_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp);
+
+void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp);
+
+void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp);
+
+inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result)
+{
+    const float angle = slerp->radians * phase;
+    const float cosine = cosf(angle);
+    const float sine = sinf(angle);
+
+    bgc_versor_set_values_fp32(
+        slerp->s0_cos_weight * cosine + slerp->s0_sin_weight * sine,
+        slerp->x1_cos_weight * cosine + slerp->x1_sin_weight * sine,
+        slerp->x2_cos_weight * cosine + slerp->x2_sin_weight * sine,
+        slerp->x3_cos_weight * cosine + slerp->x3_sin_weight * sine,
+        result
+    );
+}
+
+inline bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result)
+{
+    const double angle = slerp->radians * phase;
+    const double cosine = cos(angle);
+    const double sine = sin(angle);
+
+    bgc_versor_set_values_fp64(
+        slerp->s0_cos_weight * cosine + slerp->s0_sin_weight * sine,
+        slerp->x1_cos_weight * cosine + slerp->x1_sin_weight * sine,
+        slerp->x2_cos_weight * cosine + slerp->x2_sin_weight * sine,
+        slerp->x3_cos_weight * cosine + slerp->x3_sin_weight * sine,
+        result
+    );
+}
+
+#endif