From 222fc2b172628c4966fe78d65e9e9b1757e131c3 Mon Sep 17 00:00:00 2001
From: Julien Oculi <xdrz.jot.sr@gmail.com>
Date: Thu, 5 Jun 2025 11:00:54 +0200
Subject: [PATCH] feat: implement `maths` module

---
 maths.cpp | 39 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 39 insertions(+)
 create mode 100644 maths.cpp

diff --git a/maths.cpp b/maths.cpp
new file mode 100644
index 0000000..29afc02
--- /dev/null
+++ b/maths.cpp
@@ -0,0 +1,39 @@
+#include "math.h"
+
+//rollH atan((22.5 - 18.5) / 236) ~0.97°
+//rollV ~1°
+
+double degToRad(double deg) {
+  return deg * M_PI / 180.0;
+}
+
+double radToDeg(double rad) {
+  return rad * 180.0 / M_PI;
+}
+
+void angleToStep(long &stepX, long &stepY, double angleX, double angleY) {
+  //TODO remove magic numbers
+  double stepRatioX = M_PI / ((51.2 / 15.0)  * (100.0 * 64.0));
+  double stepRatioY = M_PI / ((48.0 / 15.0)  * (100.0 * 64.0));
+  
+  stepX = angleX / stepRatioX;
+  stepY = angleY / stepRatioY;
+}
+
+void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY, double x, double y, double z) {
+  //TODO remove magic numbers
+  double distance = 166.0; //turret to screen
+  double offsetY = 21.0; //laser to stand
+  double offsetX = 0.0; //unused
+
+  x = -x; //natural axis direction
+  double dX = x + offsetX;
+  double dY = y + offsetY;
+  double dZ = z + distance;
+
+  double rho = sqrt(pow(dX, 2.0) + pow(dY, 2.0) + pow(dZ, 2.0));
+  double angleY = M_PI / 2.0 - acos(dY / rho);
+  double angleX = atan2(dX, dZ);
+
+  angleToStep(stepX, stepY, angleX, angleY);    
+}