Skip to content

Commit

Permalink
feat: smooth mousekey with physics simulation
Browse files Browse the repository at this point in the history
  • Loading branch information
nemoto committed May 28, 2019
1 parent 66b91ea commit 4eee1d2
Show file tree
Hide file tree
Showing 2 changed files with 97 additions and 249 deletions.
321 changes: 72 additions & 249 deletions tmk_core/common/mousekey.c
View file
Expand Up @@ -23,283 +23,110 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "debug.h"
#include "mousekey.h"

inline int8_t times_inv_sqrt2(int8_t x) {
// 181/256 is pretty close to 1/sqrt(2)
// 0.70703125 0.707106781
// 1 too small for x=99 and x=198
// This ends up being a mult and discard lower 8 bits
return (x * 181) >> 8;
}

static report_mouse_t mouse_report = {0};
static void mousekey_debug(void);
static uint8_t mousekey_accel = 0;
static uint8_t mousekey_repeat = 0;
static uint16_t last_timer = 0;


typedef struct vector {
float x;
float y;
} vector_t;

#ifndef MK_3_SPEED
static report_mouse_t mouse_report = {};

typedef struct physics_state {
vector_t accel;
vector_t velocity;
} physics_state_t;

physics_state_t move_state = {{0, 0}, {0, 0}};
physics_state_t wheel_state = {{0, 0}, {0, 0}};

/*
* Mouse keys acceleration algorithm
* http://en.wikipedia.org/wiki/Mouse_keys
*
* speed = delta * max_speed * (repeat / time_to_max)**((1000+curve)/1000)
*/
/* milliseconds between the initial key press and first repeated motion event (0-2550) */
uint8_t mk_delay = MOUSEKEY_DELAY/10;
/* milliseconds between repeated motion events (0-255) */
uint8_t mk_interval = MOUSEKEY_INTERVAL;
/* steady speed (in action_delta units) applied each event (0-255) */
uint8_t mk_max_speed = MOUSEKEY_MAX_SPEED;
/* number of events (count) accelerating to steady speed (0-255) */
uint8_t mk_time_to_max = MOUSEKEY_TIME_TO_MAX;
/* ramp used to reach maximum pointer speed (NOT SUPPORTED) */
//int8_t mk_curve = 0;
/* wheel params */
uint8_t mk_wheel_max_speed = MOUSEKEY_WHEEL_MAX_SPEED;
uint8_t mk_wheel_time_to_max = MOUSEKEY_WHEEL_TIME_TO_MAX;
static void mousekey_debug(void);

static uint8_t move_unit(void) {
uint16_t unit;
if (mousekey_accel & (1<<0)) {
unit = (MOUSEKEY_MOVE_DELTA * mk_max_speed)/4;
} else if (mousekey_accel & (1<<1)) {
unit = (MOUSEKEY_MOVE_DELTA * mk_max_speed)/2;
} else if (mousekey_accel & (1<<2)) {
unit = (MOUSEKEY_MOVE_DELTA * mk_max_speed);
} else if (mousekey_repeat == 0) {
unit = MOUSEKEY_MOVE_DELTA;
} else if (mousekey_repeat >= mk_time_to_max) {
unit = MOUSEKEY_MOVE_DELTA * mk_max_speed;
} else {
unit = (MOUSEKEY_MOVE_DELTA * mk_max_speed * mousekey_repeat) / mk_time_to_max;
}
return (unit > MOUSEKEY_MOVE_MAX ? MOUSEKEY_MOVE_MAX : (unit == 0 ? 1 : unit));
static uint16_t last_timer = 0;
const float gravity=MOUSEKEY_GRAVITY;
const float dt=1.0/MOUSEKEY_FRAMERATE;
const float interval=1000.0/MOUSEKEY_FRAMERATE;

typedef struct physics_config {
float force;
float mass;
float friction;
} physics_config_t;

const physics_config_t move = { .force=MOUSEKEY_CURSOR_FORCE, .mass=MOUSEKEY_CURSOR_MASS, .friction=MOUSEKEY_CURSOR_FRICTION };
const physics_config_t wheel = { .force=MOUSEKEY_WHEEL_FORCE, .mass=MOUSEKEY_WHEEL_MASS, .friction=MOUSEKEY_WHEEL_FRICTION };

float apply_friction_1d(const physics_config_t* conf,float velocity) {
const int sign = velocity==0 ? 0 : (0<velocity ? 1 : -1);
velocity -= sign * (conf->mass) * gravity * (conf->friction) * dt;
const int afterSign= velocity == 0 ? 0 : ( 0<velocity ? 1 : -1 );
if( sign!=afterSign ){ velocity=0; }
return velocity;
}

static uint8_t wheel_unit(void) {
uint16_t unit;
if (mousekey_accel & (1<<0)) {
unit = (MOUSEKEY_WHEEL_DELTA * mk_wheel_max_speed)/4;
} else if (mousekey_accel & (1<<1)) {
unit = (MOUSEKEY_WHEEL_DELTA * mk_wheel_max_speed)/2;
} else if (mousekey_accel & (1<<2)) {
unit = (MOUSEKEY_WHEEL_DELTA * mk_wheel_max_speed);
} else if (mousekey_repeat == 0) {
unit = MOUSEKEY_WHEEL_DELTA;
} else if (mousekey_repeat >= mk_wheel_time_to_max) {
unit = MOUSEKEY_WHEEL_DELTA * mk_wheel_max_speed;
} else {
unit = (MOUSEKEY_WHEEL_DELTA * mk_wheel_max_speed * mousekey_repeat) / mk_wheel_time_to_max;
}
return (unit > MOUSEKEY_WHEEL_MAX ? MOUSEKEY_WHEEL_MAX : (unit == 0 ? 1 : unit));
void apply_friction_2d(const physics_config_t *conf, vector_t *velocity){
velocity->x = apply_friction_1d(conf,velocity->x);
velocity->y = apply_friction_1d(conf,velocity->y);
}

void mousekey_task(void) {
if (timer_elapsed(last_timer) < (mousekey_repeat ? mk_interval : mk_delay*10)) {
return;
}
if (mouse_report.x == 0 && mouse_report.y == 0 && mouse_report.v == 0 && mouse_report.h == 0) {
return;
}
if (mousekey_repeat != UINT8_MAX) mousekey_repeat++;
if (mouse_report.x > 0) mouse_report.x = move_unit();
if (mouse_report.x < 0) mouse_report.x = move_unit() * -1;
if (mouse_report.y > 0) mouse_report.y = move_unit();
if (mouse_report.y < 0) mouse_report.y = move_unit() * -1;
/* diagonal move [1/sqrt(2)] */
if (mouse_report.x && mouse_report.y) {
mouse_report.x = times_inv_sqrt2(mouse_report.x);
if (mouse_report.x == 0) { mouse_report.x = 1; }
mouse_report.y = times_inv_sqrt2(mouse_report.y);
if (mouse_report.y == 0) { mouse_report.y = 1; }
}
if (mouse_report.v > 0) mouse_report.v = wheel_unit();
if (mouse_report.v < 0) mouse_report.v = wheel_unit() * -1;
if (mouse_report.h > 0) mouse_report.h = wheel_unit();
if (mouse_report.h < 0) mouse_report.h = wheel_unit() * -1;
mousekey_send();
}

void mousekey_on(uint8_t code) {
if (code == KC_MS_UP) mouse_report.y = move_unit() * -1;
else if (code == KC_MS_DOWN) mouse_report.y = move_unit();
else if (code == KC_MS_LEFT) mouse_report.x = move_unit() * -1;
else if (code == KC_MS_RIGHT) mouse_report.x = move_unit();
else if (code == KC_MS_WH_UP) mouse_report.v = wheel_unit();
else if (code == KC_MS_WH_DOWN) mouse_report.v = wheel_unit() * -1;
else if (code == KC_MS_WH_LEFT) mouse_report.h = wheel_unit() * -1;
else if (code == KC_MS_WH_RIGHT) mouse_report.h = wheel_unit();
else if (code == KC_MS_BTN1) mouse_report.buttons |= MOUSE_BTN1;
else if (code == KC_MS_BTN2) mouse_report.buttons |= MOUSE_BTN2;
else if (code == KC_MS_BTN3) mouse_report.buttons |= MOUSE_BTN3;
else if (code == KC_MS_BTN4) mouse_report.buttons |= MOUSE_BTN4;
else if (code == KC_MS_BTN5) mouse_report.buttons |= MOUSE_BTN5;
else if (code == KC_MS_ACCEL0) mousekey_accel |= (1<<0);
else if (code == KC_MS_ACCEL1) mousekey_accel |= (1<<1);
else if (code == KC_MS_ACCEL2) mousekey_accel |= (1<<2);
}
if (timer_elapsed(last_timer) < interval)
return;

// move
{
move_state.velocity.x+=( move_state.accel.x / move.mass )*dt; // a = F/m
move_state.velocity.y+=( move_state.accel.y / move.mass )*dt;
apply_friction_2d(&move, &move_state.velocity);
mouse_report.x=move_state.velocity.x;
mouse_report.y=move_state.velocity.y;
}

// wheel
{
wheel_state.velocity.x+=( wheel_state.accel.x / wheel.mass )*dt; // a = F/m
wheel_state.velocity.y+=( wheel_state.accel.y / wheel.mass )*dt;
apply_friction_2d(&wheel, &wheel_state.velocity);
mouse_report.h=wheel_state.velocity.x;
mouse_report.v=wheel_state.velocity.y;
}

void mousekey_off(uint8_t code) {
if (code == KC_MS_UP && mouse_report.y < 0) mouse_report.y = 0;
else if (code == KC_MS_DOWN && mouse_report.y > 0) mouse_report.y = 0;
else if (code == KC_MS_LEFT && mouse_report.x < 0) mouse_report.x = 0;
else if (code == KC_MS_RIGHT && mouse_report.x > 0) mouse_report.x = 0;
else if (code == KC_MS_WH_UP && mouse_report.v > 0) mouse_report.v = 0;
else if (code == KC_MS_WH_DOWN && mouse_report.v < 0) mouse_report.v = 0;
else if (code == KC_MS_WH_LEFT && mouse_report.h < 0) mouse_report.h = 0;
else if (code == KC_MS_WH_RIGHT && mouse_report.h > 0) mouse_report.h = 0;
else if (code == KC_MS_BTN1) mouse_report.buttons &= ~MOUSE_BTN1;
else if (code == KC_MS_BTN2) mouse_report.buttons &= ~MOUSE_BTN2;
else if (code == KC_MS_BTN3) mouse_report.buttons &= ~MOUSE_BTN3;
else if (code == KC_MS_BTN4) mouse_report.buttons &= ~MOUSE_BTN4;
else if (code == KC_MS_BTN5) mouse_report.buttons &= ~MOUSE_BTN5;
else if (code == KC_MS_ACCEL0) mousekey_accel &= ~(1<<0);
else if (code == KC_MS_ACCEL1) mousekey_accel &= ~(1<<1);
else if (code == KC_MS_ACCEL2) mousekey_accel &= ~(1<<2);
if (mouse_report.x == 0 && mouse_report.y == 0 && mouse_report.v == 0 && mouse_report.h == 0)
mousekey_repeat = 0;
}




#else /* #ifndef MK_3_SPEED */



enum {
mkspd_unmod,
mkspd_0,
mkspd_1,
mkspd_2,
mkspd_COUNT
};
#ifndef MK_MOMENTARY_ACCEL
static uint8_t mk_speed = mkspd_1;
#else
static uint8_t mk_speed = mkspd_unmod;
static uint8_t mkspd_DEFAULT = mkspd_unmod;
#endif
static uint16_t last_timer_c = 0;
static uint16_t last_timer_w = 0;
uint16_t c_offsets[mkspd_COUNT] = {
MK_C_OFFSET_UNMOD, MK_C_OFFSET_0, MK_C_OFFSET_1, MK_C_OFFSET_2
};
uint16_t c_intervals[mkspd_COUNT] = {
MK_C_INTERVAL_UNMOD, MK_C_INTERVAL_0, MK_C_INTERVAL_1, MK_C_INTERVAL_2
};
uint16_t w_offsets[mkspd_COUNT] = {
MK_W_OFFSET_UNMOD, MK_W_OFFSET_0, MK_W_OFFSET_1, MK_W_OFFSET_2
};
uint16_t w_intervals[mkspd_COUNT] = {
MK_W_INTERVAL_UNMOD, MK_W_INTERVAL_0, MK_W_INTERVAL_1, MK_W_INTERVAL_2
};

return;

void mousekey_task(void) {
// report cursor and scroll movement independently
report_mouse_t const tmpmr = mouse_report;
if ((mouse_report.x || mouse_report.y) && timer_elapsed(last_timer_c) > c_intervals[mk_speed]) {
mouse_report.h = 0;
mouse_report.v = 0;
mousekey_send();
last_timer_c = last_timer;
mouse_report = tmpmr;
}
if ((mouse_report.h || mouse_report.v) && timer_elapsed(last_timer_w) > w_intervals[mk_speed]) {
mouse_report.x = 0;
mouse_report.y = 0;
mousekey_send();
last_timer_w = last_timer;
mouse_report = tmpmr;
}
}

void adjust_speed(void) {
uint16_t const c_offset = c_offsets[mk_speed];
uint16_t const w_offset = w_offsets[mk_speed];
if (mouse_report.x > 0) mouse_report.x = c_offset;
if (mouse_report.x < 0) mouse_report.x = c_offset * -1;
if (mouse_report.y > 0) mouse_report.y = c_offset;
if (mouse_report.y < 0) mouse_report.y = c_offset * -1;
if (mouse_report.h > 0) mouse_report.h = w_offset;
if (mouse_report.h < 0) mouse_report.h = w_offset * -1;
if (mouse_report.v > 0) mouse_report.v = w_offset;
if (mouse_report.v < 0) mouse_report.v = w_offset * -1;
// adjust for diagonals
if (mouse_report.x && mouse_report.y) {
mouse_report.x = times_inv_sqrt2(mouse_report.x);
if (mouse_report.x == 0) { mouse_report.x = 1; }
mouse_report.y = times_inv_sqrt2(mouse_report.y);
if (mouse_report.y == 0) { mouse_report.y = 1; }
}
if (mouse_report.h && mouse_report.v) {
mouse_report.h = times_inv_sqrt2(mouse_report.h);
mouse_report.v = times_inv_sqrt2(mouse_report.v);
}
}

void mousekey_on(uint8_t code) {
uint16_t const c_offset = c_offsets[mk_speed];
uint16_t const w_offset = w_offsets[mk_speed];
uint8_t const old_speed = mk_speed;
if (code == KC_MS_UP) mouse_report.y = c_offset * -1;
else if (code == KC_MS_DOWN) mouse_report.y = c_offset;
else if (code == KC_MS_LEFT) mouse_report.x = c_offset * -1;
else if (code == KC_MS_RIGHT) mouse_report.x = c_offset;
else if (code == KC_MS_WH_UP) mouse_report.v = w_offset;
else if (code == KC_MS_WH_DOWN) mouse_report.v = w_offset * -1;
else if (code == KC_MS_WH_LEFT) mouse_report.h = w_offset * -1;
else if (code == KC_MS_WH_RIGHT) mouse_report.h = w_offset;
if (code == KC_MS_UP) move_state.accel.y = -move.force;
else if (code == KC_MS_DOWN) move_state.accel.y = move.force;
else if (code == KC_MS_LEFT) move_state.accel.x = -move.force;
else if (code == KC_MS_RIGHT) move_state.accel.x = move.force;
else if (code == KC_MS_WH_UP) wheel_state.accel.y = wheel.force;
else if (code == KC_MS_WH_DOWN) wheel_state.accel.y = -wheel.force;
else if (code == KC_MS_WH_LEFT) wheel_state.accel.x = wheel.force;
else if (code == KC_MS_WH_RIGHT) wheel_state.accel.x = -wheel.force;
else if (code == KC_MS_BTN1) mouse_report.buttons |= MOUSE_BTN1;
else if (code == KC_MS_BTN2) mouse_report.buttons |= MOUSE_BTN2;
else if (code == KC_MS_BTN3) mouse_report.buttons |= MOUSE_BTN3;
else if (code == KC_MS_BTN4) mouse_report.buttons |= MOUSE_BTN4;
else if (code == KC_MS_BTN5) mouse_report.buttons |= MOUSE_BTN5;
else if (code == KC_MS_ACCEL0) mk_speed = mkspd_0;
else if (code == KC_MS_ACCEL1) mk_speed = mkspd_1;
else if (code == KC_MS_ACCEL2) mk_speed = mkspd_2;
if (mk_speed != old_speed) adjust_speed();
}

void mousekey_off(uint8_t code) {
#ifdef MK_MOMENTARY_ACCEL
uint8_t const old_speed = mk_speed;
#endif
if (code == KC_MS_UP && mouse_report.y < 0) mouse_report.y = 0;
else if (code == KC_MS_DOWN && mouse_report.y > 0) mouse_report.y = 0;
else if (code == KC_MS_LEFT && mouse_report.x < 0) mouse_report.x = 0;
else if (code == KC_MS_RIGHT && mouse_report.x > 0) mouse_report.x = 0;
else if (code == KC_MS_WH_UP && mouse_report.v > 0) mouse_report.v = 0;
else if (code == KC_MS_WH_DOWN && mouse_report.v < 0) mouse_report.v = 0;
else if (code == KC_MS_WH_LEFT && mouse_report.h < 0) mouse_report.h = 0;
else if (code == KC_MS_WH_RIGHT && mouse_report.h > 0) mouse_report.h = 0;
if (code == KC_MS_UP && move_state.accel.y < 0) move_state.accel.y = 0;
else if (code == KC_MS_DOWN && move_state.accel.y > 0) move_state.accel.y = 0;
else if (code == KC_MS_LEFT && move_state.accel.x < 0) move_state.accel.x = 0;
else if (code == KC_MS_RIGHT && move_state.accel.x > 0) move_state.accel.x = 0;
else if (code == KC_MS_WH_UP && wheel_state.accel.y > 0) wheel_state.accel.y = 0;
else if (code == KC_MS_WH_DOWN && wheel_state.accel.y < 0) wheel_state.accel.y = 0;
else if (code == KC_MS_WH_LEFT && wheel_state.accel.x < 0) wheel_state.accel.x = 0;
else if (code == KC_MS_WH_RIGHT && wheel_state.accel.x > 0) wheel_state.accel.x = 0;
else if (code == KC_MS_BTN1) mouse_report.buttons &= ~MOUSE_BTN1;
else if (code == KC_MS_BTN2) mouse_report.buttons &= ~MOUSE_BTN2;
else if (code == KC_MS_BTN3) mouse_report.buttons &= ~MOUSE_BTN3;
else if (code == KC_MS_BTN4) mouse_report.buttons &= ~MOUSE_BTN4;
else if (code == KC_MS_BTN5) mouse_report.buttons &= ~MOUSE_BTN5;
#ifdef MK_MOMENTARY_ACCEL
else if (code == KC_MS_ACCEL0) mk_speed = mkspd_DEFAULT;
else if (code == KC_MS_ACCEL1) mk_speed = mkspd_DEFAULT;
else if (code == KC_MS_ACCEL2) mk_speed = mkspd_DEFAULT;
if (mk_speed != old_speed) adjust_speed();
#endif
}




#endif /* #ifndef MK_3_SPEED */




void mousekey_send(void) {
mousekey_debug();
host_mouse_send(&mouse_report);
Expand All @@ -308,8 +135,6 @@ void mousekey_send(void) {

void mousekey_clear(void) {
mouse_report = (report_mouse_t){};
mousekey_repeat = 0;
mousekey_accel = 0;
}

static void mousekey_debug(void) {
Expand All @@ -320,6 +145,4 @@ static void mousekey_debug(void) {
print_decs(mouse_report.y); print(" ");
print_decs(mouse_report.v); print(" ");
print_decs(mouse_report.h); print("](");
print_dec(mousekey_repeat); print("/");
print_dec(mousekey_accel); print(")\n");
}

0 comments on commit 4eee1d2

Please sign in to comment.