From 5ecb2eb9b529c1ad87eff4a8540d8ca1825b1f27 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 3 Aug 2024 11:47:55 +0200
Subject: [PATCH] =?UTF-8?q?=E2=9A=96=EF=B8=8F=20Adds=20equality=20function?=
 =?UTF-8?q?s?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/lib/ESP32-sveltekit/Kinematics.h    | 16 +++++++++++-----
 esp32/lib/ESP32-sveltekit/MathUtils.h     | 17 +++++++++++++++--
 esp32/lib/ESP32-sveltekit/MotionService.h |  2 +-
 3 files changed, 27 insertions(+), 8 deletions(-)

diff --git a/esp32/lib/ESP32-sveltekit/Kinematics.h b/esp32/lib/ESP32-sveltekit/Kinematics.h
index 5c838e2..2710066 100644
--- a/esp32/lib/ESP32-sveltekit/Kinematics.h
+++ b/esp32/lib/ESP32-sveltekit/Kinematics.h
@@ -3,7 +3,6 @@
 
 #include <MathUtils.h>
 
-
 struct body_state_t {
     float omega, phi, psi, xm, ym, zm;
     float feet[4][4];
@@ -11,11 +10,12 @@ struct body_state_t {
     void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); }
 
     bool isEqual(const body_state_t &other) const {
-        if (omega != other.omega || phi != other.phi || psi != other.psi || xm != other.xm || ym != other.ym ||
-            zm != other.zm) {
+        if (!IS_ALMOST_EQUAL(omega, other.omega) || !IS_ALMOST_EQUAL(phi, other.phi) ||
+            !IS_ALMOST_EQUAL(psi, other.psi) || !IS_ALMOST_EQUAL(xm, other.xm) || !IS_ALMOST_EQUAL(ym, other.ym) ||
+            !IS_ALMOST_EQUAL(zm, other.zm)) {
             return false;
         }
-        return ARRAY_EQUAL(feet, other.feet);
+        return arrayEqual(feet, other.feet, 0.1);
     }
 };
 
@@ -67,7 +67,13 @@ class Kinematics {
         if (currentState.isEqual(body_state)) return ESP_OK;
 
         ret = bodyIK(body_state);
-        currentState = body_state;
+        currentState.omega = body_state.omega;
+        currentState.phi = body_state.phi;
+        currentState.psi = body_state.psi;
+        currentState.xm = body_state.xm;
+        currentState.ym = body_state.ym;
+        currentState.zm = body_state.zm;
+        currentState.updateFeet(body_state.feet);
 
         ret += inverse(Tlf, inv);
         MAT_MULT(inv, body_state.feet[0], point, 4, 4, 1);
diff --git a/esp32/lib/ESP32-sveltekit/MathUtils.h b/esp32/lib/ESP32-sveltekit/MathUtils.h
index 1011dad..b7c5283 100644
--- a/esp32/lib/ESP32-sveltekit/MathUtils.h
+++ b/esp32/lib/ESP32-sveltekit/MathUtils.h
@@ -6,8 +6,6 @@
 
 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof(arr[0]))
 
-#define ARRAY_EQUAL(arr1, arr2) (memcmp((arr1), (arr2), sizeof(arr1)) == 0)
-
 #define COPY_2D_ARRAY_4x4(dest, src) \
     do {                             \
         (dest)[0][0] = (src)[0][0];  \
@@ -50,10 +48,25 @@
 
 #define SIN_DEG_F(deg) (sinf(DEG_TO_RAD_F(deg)))
 
+#define IS_EQUAL(a, b, epsilon) (std::fabs((a) - (b)) < (epsilon))
+
+#define IS_ALMOST_EQUAL(a, b) IS_EQUAL((a), (b), 0.001f)
+
 inline float lerp(float start, float end, float t) { return (1 - t) * start + t * end; }
 
 inline bool isEqual(float a, float b, float epsilon) { return std::fabs(a - b) < epsilon; }
 
 inline float round2(float value) { return (int)(value * 100 + 0.5) / 100.0; }
 
+inline bool arrayEqual(const float arr1[4][4], const float arr2[4][4], float epsilon = 1e-3) {
+    for (int i = 0; i < 4; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            if (std::fabs(arr1[i][j] - arr2[i][j]) > epsilon) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
 #endif
\ No newline at end of file
diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
index 5bca11c..eb7b6c8 100644
--- a/esp32/lib/ESP32-sveltekit/MotionService.h
+++ b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -113,7 +113,7 @@ class MotionService {
         bool updated = false;
         for (int i = 0; i < 12; i++) {
             float new_angle = useLerp ? lerp(angles[i], new_angles[i] * dir[i], 0.3) : new_angles[i] * dir[i];
-            if (new_angle != angles[i]) {
+            if (!isEqual(new_angle, angles[i], 0.1)) {
                 angles[i] = new_angle;
                 updated = true;
             }