From 4e375a70f1bdd4073a8ce015b2195bf2d40961eb Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@tutanota.com>
Date: Wed, 31 Jul 2024 21:21:01 +0200
Subject: [PATCH] optimization: collapse function calls into sistema_discr

at the expense of "modularity" but don't really need it at this point
---
 diffdrive.m        | 3 ---
 diffdrive_to_uni.m | 2 +-
 sistema.m          | 3 ---
 sistema_discr.m    | 4 +++-
 unicycle.m         | 7 -------
 5 files changed, 4 insertions(+), 15 deletions(-)
 delete mode 100644 diffdrive.m
 delete mode 100644 sistema.m
 delete mode 100644 unicycle.m

diff --git a/diffdrive.m b/diffdrive.m
deleted file mode 100644
index ef3899f..0000000
--- a/diffdrive.m
+++ /dev/null
@@ -1,3 +0,0 @@
-function dq = diffdrive(t, q, u, sim_data)
-    dq = unicycle(t, q, diffdrive_to_uni(u, sim_data));
-end
diff --git a/diffdrive_to_uni.m b/diffdrive_to_uni.m
index 08b7305..863c0d8 100644
--- a/diffdrive_to_uni.m
+++ b/diffdrive_to_uni.m
@@ -1,3 +1,3 @@
 function u_ = diffdrive_to_uni(u, sim_data)
-    u_ = [sim_data.r/2, sim_data.r/2 ; sim_data.r/sim_data.d -sim_data.r/sim_data.d] * u;
+    u_ = [sim_data.r/2, sim_data.r/2 ; sim_data.r/sim_data.d, -sim_data.r/sim_data.d] * u;
 end
diff --git a/sistema.m b/sistema.m
deleted file mode 100644
index aee1e93..0000000
--- a/sistema.m
+++ /dev/null
@@ -1,3 +0,0 @@
-function q = sistema(t, q, sim_data)
-    q = unicycle(t, q, control_act(t, q, sim_data));
-end
diff --git a/sistema_discr.m b/sistema_discr.m
index edae3f7..644f7ec 100644
--- a/sistema_discr.m
+++ b/sistema_discr.m
@@ -1,3 +1,5 @@
 function dq = sistema_discr(t, q, u_discr, sim_data)
-    dq = diffdrive(t, q, u_discr, sim_data);
+    theta = q(3);
+    G_q = [cos(theta), 0; sin(theta), 0; 0, 1];
+    dq = G_q*diffdrive_to_uni(u_discr, sim_data);
 end
diff --git a/unicycle.m b/unicycle.m
deleted file mode 100644
index 32475ee..0000000
--- a/unicycle.m
+++ /dev/null
@@ -1,7 +0,0 @@
-function dq = unicycle(t, q, u)
-    % u is (v;w)
-    % x is (x; y; theta)
-    theta = q(3);
-    G_q = [cos(theta), 0; sin(theta), 0; 0, 1];
-    dq = G_q*u;
-end