From 2de123840504ff3a9638d27e49a44ed0ebccb683 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Sat, 9 Nov 2024 16:24:09 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=8D=20Moves=20static=20definitions=20o?= =?UTF-8?q?f=20bezier=20steps=20and=20height?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/lib/ESP32-sveltekit/Gait/GaitState.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h index fe96c80..3eb0464 100644 --- a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h +++ b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h @@ -228,6 +228,12 @@ class BezierState : public GaitState { combinatorial_constexpr(11, 11) // 1 }; + alignas(32) static constexpr float BEZIER_STEPS[12] = {-1.0f, -1.4f, -1.5f, -1.5f, -1.5f, 0.0f, + 0.0f, 0.0f, 1.5f, 1.5f, 1.4f, 1.0f}; + + alignas(32) static constexpr float BEZIER_HEIGHTS[12] = {0.0f, 0.0f, 0.9f, 0.9f, 0.9f, 0.9f, + 0.9f, 1.1f, 1.1f, 1.1f, 0.0f, 0.0f}; + protected: const char *name() const override { return "Bezier"; } @@ -245,8 +251,7 @@ class BezierState : public GaitState { phase_time += dt * gait_state.step_velocity * 2; if (phase_time >= 1.0f) { - phase += 1; - phase %= 2; + phase ^= 1; phase_time = 0; } } @@ -310,20 +315,15 @@ class BezierState : public GaitState { float X_POLAR = std::cos(angle); float Z_POLAR = std::sin(angle); - float STEP[] = {-length, -length * 1.4f, -length * 1.5f, -length * 1.5f, -length * 1.5f, 0.0f, - 0.0f, 0.0f, length * 1.5f, length * 1.5f, length * 1.4f, length}; - float Y[] = {0.0f, 0.0f, 0.9f * *height, 0.9f * *height, 0.9f * *height, 0.9f * *height, - 0.9f * *height, 1.1f * *height, 1.1f * *height, 1.1f * *height, 0.0f, 0.0f}; - float phase_power = 1.0f; float inv_phase_power = std::pow(1.0f - phase, 11); float one_minus_phase = 1.0f - phase; for (int i = 0; i < 12; i++) { float b = COMBINATORIAL_VALUES[i] * phase_power * inv_phase_power; - point[0] += b * STEP[i] * X_POLAR; - point[1] += b * Y[i]; - point[2] += b * STEP[i] * Z_POLAR; + point[0] += b * BEZIER_STEPS[i] * length * X_POLAR; + point[1] += b * BEZIER_HEIGHTS[i] * *height; + point[2] += b * BEZIER_STEPS[i] * length * Z_POLAR; phase_power *= phase; inv_phase_power /= one_minus_phase;