From 634b3292b4dc6e77795599d7102906260f6a6d1c Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 9 Nov 2024 14:14:47 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=A5=BD=20Makes=20use=20of=20Atan2=20to=20?=
 =?UTF-8?q?calculate=20lateral=20fraction?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/gait.ts                        | 11 +++++------
 esp32/lib/ESP32-sveltekit/Gait/GaitState.h |  3 +--
 2 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts
index 79fb3b2..2fa7946 100644
--- a/app/src/lib/gait.ts
+++ b/app/src/lib/gait.ts
@@ -289,7 +289,6 @@ export class BezierState extends GaitState {
         this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2);
         if (this.gait_state.step_x < 0) {
             this.step_length = -this.step_length;
-            this.gait_state.step_z = -this.gait_state.step_z;
         }
         this.update_phase();
         this.update_feet_positions();
@@ -335,7 +334,7 @@ export class BezierState extends GaitState {
         ...args: number[]
     ) {
         let length = this.step_length / 2;
-        let angle = (this.gait_state.step_z * Math.PI) / 2;
+        let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2;
         const delta_pos = controller(length, angle, ...args, this.phase);
 
         length = this.gait_state.step_angle * 2;
@@ -358,13 +357,13 @@ const stance_curve = (length: number, angle: number, depth: number, phase: numbe
 
     const step = length * (1 - 2 * phase);
     const X = step * X_POLAR;
-    const Y = step * Y_POLAR;
-    let Z = 0;
+    const Z = step * Y_POLAR;
+    let Y = 0;
 
     if (length !== 0) {
-        Z = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length));
+        Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length));
     }
-    return [X, Z, Y];
+    return [X, Y, Z];
 };
 
 const yawArc = (default_foot_pos: number[], current_foot_pos: number[]): number => {
diff --git a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
index 7693e3f..b220712 100644
--- a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
+++ b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
@@ -220,7 +220,6 @@ class BezierState : public GaitState {
         step_length = std::sqrt(gait_state.step_x * gait_state.step_x + gait_state.step_z * gait_state.step_z);
         if (gait_state.step_x < 0.0f) {
             step_length = -step_length;
-            gait_state.step_z = -gait_state.step_z;
         }
         updatePhase(dt);
         updateFeetPositions(body_state);
@@ -261,7 +260,7 @@ class BezierState : public GaitState {
     void controller(const int index, body_state_t &body_state,
                     std::function<void(float, float, float *, float, float *)> curve, float *arg) {
         float length = step_length / 2.0f;
-        float angle = gait_state.step_z * M_PI_2;
+        float angle = std::atan2(gait_state.step_z, step_length) * 2;
         float point[3] = {0, 0, 0};
         curve(length, angle, arg, phase_time, point);