From ea42cc0aaca427c625022bbf90a43f0fb9bbb352 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 9 Nov 2024 16:03:14 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=AB=9A=20Makes=20combinatorial=20a=20cons?=
 =?UTF-8?q?tant=20expression?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/lib/ESP32-sveltekit/Gait/GaitState.h | 31 +++++++++++++---------
 esp32/lib/ESP32-sveltekit/MathUtils.h      | 12 +++++++++
 2 files changed, 30 insertions(+), 13 deletions(-)

diff --git a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
index 597a22a..499ff5f 100644
--- a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
+++ b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
@@ -1,4 +1,5 @@
 #include <Kinematics.h>
+#include <MathUtils.h>
 
 struct gait_state_t {
     float step_height;
@@ -209,8 +210,23 @@ class BezierState : public GaitState {
   private:
     float phase_time = 0.0f;
     uint8_t phase = 0;
-    uint8_t contact_phases[4][2] = {{1, 0}, {0, 1}, {0, 1}, {1, 0}};
+    static constexpr uint8_t contact_phases[4][2] = {{1, 0}, {0, 1}, {0, 1}, {1, 0}};
+    static constexpr uint8_t BEZIER_POINTS = 12;
     float step_length = 0.0f;
+    static constexpr std::array<float, BEZIER_POINTS> COMBINATORIAL_VALUES = {
+        combinatorial_constexpr(11, 0),  // 1
+        combinatorial_constexpr(11, 1),  // 11
+        combinatorial_constexpr(11, 2),  // 55
+        combinatorial_constexpr(11, 3),  // 165
+        combinatorial_constexpr(11, 4),  // 330
+        combinatorial_constexpr(11, 5),  // 462
+        combinatorial_constexpr(11, 6),  // 462
+        combinatorial_constexpr(11, 7),  // 330
+        combinatorial_constexpr(11, 8),  // 165
+        combinatorial_constexpr(11, 9),  // 55
+        combinatorial_constexpr(11, 10), // 11
+        combinatorial_constexpr(11, 11)  // 1
+    };
 
   protected:
     const char *name() const override { return "Bezier"; }
@@ -300,7 +316,7 @@ class BezierState : public GaitState {
                      0.9f * *height, 1.1f * *height, 1.1f * *height, 1.1f * *height, 0.0f,           0.0f};
 
         for (int i = 0; i < 12; i++) {
-            float b = combinatorial(11, i) * std::pow(phase, i) * std::pow(1.0f - phase, 11 - i);
+            float b = COMBINATORIAL_VALUES[i] * std::pow(phase, i) * std::pow(1.0f - phase, 11 - i);
             point[0] += b * STEP[i] * X_POLAR;
             point[1] += b * Y[i];
             point[2] += b * STEP[i] * Z_POLAR;
@@ -316,15 +332,4 @@ class BezierState : public GaitState {
 
         return M_PI_2 + foot_dir + offset_mod;
     }
-
-    static float combinatorial(const int n, int k) {
-        if (k < 0 || k > n) return 0.0f;
-        if (k == 0 || k == n) return 1.0f;
-        k = std::min(k, n - k);
-        float c = 1.0f;
-        for (int i = 0; i < k; i++) {
-            c = (c * (n - i)) / (i + 1);
-        }
-        return c;
-    }
 };
\ No newline at end of file
diff --git a/esp32/lib/ESP32-sveltekit/MathUtils.h b/esp32/lib/ESP32-sveltekit/MathUtils.h
index b7c5283..182a378 100644
--- a/esp32/lib/ESP32-sveltekit/MathUtils.h
+++ b/esp32/lib/ESP32-sveltekit/MathUtils.h
@@ -69,4 +69,16 @@ inline bool arrayEqual(const float arr1[4][4], const float arr2[4][4], float eps
     return true;
 }
 
+static constexpr float combinatorial_constexpr(const int n, int k) {
+    if (k < 0 || k > n) return 0.0f;
+    if (k == 0 || k == n) return 1.0f;
+    k = (k < (n - k)) ? k : (n - k);
+    float result = 1.0f;
+    for (int i = 0; i < k; ++i) {
+        result *= (n - i);
+        result /= (i + 1);
+    }
+    return result;
+}
+
 #endif
\ No newline at end of file