import upstream maccel baseline

2026-03-24 12:10:31 +00:00
parent 6e948d7b39
commit 5f1254d11a
108 changed files with 18930 additions and 0 deletions
@@ -0,0 +1,140 @@
+#ifndef _ACCEL_H_
+#define _ACCEL_H_
+
+#include "accel/linear.h"
+#include "accel/mode.h"
+#include "accel/natural.h"
+#include "accel/synchronous.h"
+#include "dbg.h"
+#include "fixedptc.h"
+#include "math.h"
+#include "speed.h"
+
+struct no_accel_curve_args {};
+
+union __accel_args {
+  struct natural_curve_args natural;
+  struct linear_curve_args linear;
+  struct synchronous_curve_args synchronous;
+  struct no_accel_curve_args no_accel;
+};
+
+struct accel_args {
+  fpt sens_mult;
+  fpt yx_ratio;
+  fpt input_dpi;
+  fpt angle_rotation_deg;
+
+  enum accel_mode tag;
+  union __accel_args args;
+};
+
+const fpt NORMALIZED_DPI = fpt_fromint(1000);
+
+/**
+ * Calculate the factor by which to multiply the input vector
+ * in order to get the desired output speed.
+ *
+ */
+static inline struct vector sensitivity(fpt input_speed,
+                                        struct accel_args args) {
+  fpt sens;
+
+  switch (args.tag) {
+  case synchronous:
+    dbg("accel mode %d: synchronous", args.tag);
+    sens = __synchronous_sens_fun(input_speed, args.args.synchronous);
+    break;
+  case natural:
+    dbg("accel mode %d: natural", args.tag);
+    sens = __natural_sens_fun(input_speed, args.args.natural);
+    break;
+  case linear:
+    dbg("accel mode %d: linear", args.tag);
+    sens = __linear_sens_fun(input_speed, args.args.linear);
+    break;
+  case no_accel:
+    dbg("accel mode %d: no_accel", args.tag);
+    sens = FIXEDPT_ONE;
+    break;
+  default:
+    sens = FIXEDPT_ONE;
+  }
+  sens = fpt_mul(sens, args.sens_mult);
+  return (struct vector){sens, fpt_mul(sens, args.yx_ratio)};
+}
+
+const fpt DEG_TO_RAD_FACTOR = fpt_xdiv(FIXEDPT_PI, fpt_rconst(180));
+
+static inline void f_accelerate(int *x, int *y, fpt time_interval_ms,
+                                struct accel_args args) {
+  static fpt carry_x = 0;
+  static fpt carry_y = 0;
+
+  fpt dx = fpt_fromint(*x);
+  fpt dy = fpt_fromint(*y);
+
+  {
+    if (args.angle_rotation_deg == 0) {
+      goto accel_routine;
+    }
+    // Add rotation of input vector
+    fpt degrees = args.angle_rotation_deg;
+    fpt radians =
+        fpt_mul(degrees, DEG_TO_RAD_FACTOR); // Convert degrees to radians
+
+    fpt cos_angle = fpt_cos(radians);
+    fpt sin_angle = fpt_sin(radians);
+
+    dbg("rotation angle(deg):     %s deg", fptoa(degrees));
+    dbg("rotation angle(rad):     %s rad", fptoa(radians));
+    dbg("cosine of rotation:      %s", fptoa(cos_angle));
+    dbg("sine of rotation:        %s", fptoa(sin_angle));
+
+    // Rotate input vector
+    fpt dx_rot = fpt_mul(dx, cos_angle) - fpt_mul(dy, sin_angle);
+    fpt dy_rot = fpt_mul(dx, sin_angle) + fpt_mul(dy, cos_angle);
+
+    dbg("rotated x:               %s", fptoa(dx_rot));
+    dbg("rotated y:               %s", fptoa(dy_rot));
+
+    dx = dx_rot;
+    dy = dy_rot;
+  }
+accel_routine:
+
+  dbg("in                        (%d, %d)", *x, *y);
+  dbg("in: x (fpt conversion) %s", fptoa(dx));
+  dbg("in: y (fpt conversion) %s", fptoa(dy));
+
+  fpt dpi_factor = fpt_div(NORMALIZED_DPI, args.input_dpi);
+  dbg("dpi adjustment factor:     %s", fptoa(dpi_factor));
+  dx = fpt_mul(dx, dpi_factor);
+  dy = fpt_mul(dy, dpi_factor);
+
+  fpt speed_in = input_speed(dx, dy, time_interval_ms);
+  struct vector sens = sensitivity(speed_in, args);
+  dbg("scale x                    %s", fptoa(sens.x));
+  dbg("scale y                    %s", fptoa(sens.y));
+
+  fpt dx_out = fpt_mul(dx, sens.x);
+  fpt dy_out = fpt_mul(dy, sens.y);
+
+  dx_out = fpt_add(dx_out, carry_x);
+  dy_out = fpt_add(dy_out, carry_y);
+
+  dbg("out: x                     %s", fptoa(dx_out));
+  dbg("out: y                     %s", fptoa(dy_out));
+
+  *x = fpt_toint(dx_out);
+  *y = fpt_toint(dy_out);
+
+  dbg("out (int conversion)      (%d, %d)", *x, *y);
+
+  carry_x = fpt_sub(dx_out, fpt_fromint(*x));
+  carry_y = fpt_sub(dy_out, fpt_fromint(*y));
+
+  dbg("carry                     (%s, %s)", fptoa(carry_x), fptoa(carry_x));
+}
+
+#endif