From 748d094c9cb6a5f77d9d02853deef49f976b4446 Mon Sep 17 00:00:00 2001 From: Peter Hutterer Date: Tue, 26 Aug 2014 09:37:29 +1000 Subject: [PATCH] util: add a couple of 3x3 matrix helper functions Signed-off-by: Peter Hutterer Reviewed-by: Hans de Goede --- src/libinput-util.h | 100 ++++++++++++++++++++++++++++++++++++++++++++ test/misc.c | 78 ++++++++++++++++++++++++++++++++++ 2 files changed, 178 insertions(+) diff --git a/src/libinput-util.h b/src/libinput-util.h index 5d366b02..f8072108 100644 --- a/src/libinput-util.h +++ b/src/libinput-util.h @@ -25,6 +25,7 @@ #include #include +#include #include "libinput.h" @@ -179,4 +180,103 @@ long_set_bit_state(unsigned long *array, int bit, int state) long_clear_bit(array, bit); } +struct matrix { + float val[3][3]; /* [row][col] */ +}; + +static inline void +matrix_init_identity(struct matrix *m) +{ + memset(m, 0, sizeof(*m)); + m->val[0][0] = 1; + m->val[1][1] = 1; + m->val[2][2] = 1; +} + +static inline void +matrix_from_farray6(struct matrix *m, const float values[6]) +{ + matrix_init_identity(m); + m->val[0][0] = values[0]; + m->val[0][1] = values[1]; + m->val[0][2] = values[2]; + m->val[1][0] = values[3]; + m->val[1][1] = values[4]; + m->val[1][2] = values[5]; +} + +static inline void +matrix_init_scale(struct matrix *m, float sx, float sy) +{ + matrix_init_identity(m); + m->val[0][0] = sx; + m->val[1][1] = sy; +} + +static inline void +matrix_init_translate(struct matrix *m, float x, float y) +{ + matrix_init_identity(m); + m->val[0][2] = x; + m->val[1][2] = y; +} + +static inline int +matrix_is_identity(struct matrix *m) +{ + return (m->val[0][0] == 1 && + m->val[0][1] == 0 && + m->val[0][2] == 0 && + m->val[1][0] == 0 && + m->val[1][1] == 1 && + m->val[1][2] == 0 && + m->val[2][0] == 0 && + m->val[2][1] == 0 && + m->val[2][2] == 1); +} + +static inline void +matrix_mult(struct matrix *dest, + const struct matrix *m1, + const struct matrix *m2) +{ + struct matrix m; /* allow for dest == m1 or dest == m2 */ + int row, col, i; + + for (row = 0; row < 3; row++) { + for (col = 0; col < 3; col++) { + double v = 0; + for (i = 0; i < 3; i++) { + v += m1->val[row][i] * m2->val[i][col]; + } + m.val[row][col] = v; + } + } + + memcpy(dest, &m, sizeof(m)); +} + +static inline void +matrix_mult_vec(struct matrix *m, int *x, int *y) +{ + int tx, ty; + + tx = *x * m->val[0][0] + *y * m->val[0][1] + m->val[0][2]; + ty = *x * m->val[1][0] + *y * m->val[1][1] + m->val[1][2]; + + *x = tx; + *y = ty; +} + +static inline void +matrix_to_farray6(const struct matrix *m, float out[6]) +{ + out[0] = m->val[0][0]; + out[1] = m->val[0][1]; + out[2] = m->val[0][2]; + out[3] = m->val[1][0]; + out[4] = m->val[1][1]; + out[5] = m->val[1][2]; +} + #endif /* LIBINPUT_UTIL_H */ diff --git a/test/misc.c b/test/misc.c index 983c3f9b..1512180e 100644 --- a/test/misc.c +++ b/test/misc.c @@ -431,6 +431,83 @@ START_TEST(config_status_string) } END_TEST +START_TEST(matrix_helpers) +{ + struct matrix m1, m2, m3; + float f[6] = { 1, 2, 3, 4, 5, 6 }; + int x, y; + int row, col; + + matrix_init_identity(&m1); + + for (row = 0; row < 3; row++) { + for (col = 0; col < 3; col++) { + ck_assert_int_eq(m1.val[row][col], + (row == col) ? 1 : 0); + } + } + ck_assert(matrix_is_identity(&m1)); + + matrix_from_farray6(&m2, f); + ck_assert_int_eq(m2.val[0][0], 1); + ck_assert_int_eq(m2.val[0][1], 2); + ck_assert_int_eq(m2.val[0][2], 3); + ck_assert_int_eq(m2.val[1][0], 4); + ck_assert_int_eq(m2.val[1][1], 5); + ck_assert_int_eq(m2.val[1][2], 6); + ck_assert_int_eq(m2.val[2][0], 0); + ck_assert_int_eq(m2.val[2][1], 0); + ck_assert_int_eq(m2.val[2][2], 1); + + x = 100; + y = 5; + matrix_mult_vec(&m1, &x, &y); + ck_assert_int_eq(x, 100); + ck_assert_int_eq(y, 5); + + matrix_mult(&m3, &m1, &m1); + ck_assert(matrix_is_identity(&m3)); + + matrix_init_scale(&m2, 2, 4); + ck_assert_int_eq(m2.val[0][0], 2); + ck_assert_int_eq(m2.val[0][1], 0); + ck_assert_int_eq(m2.val[0][2], 0); + ck_assert_int_eq(m2.val[1][0], 0); + ck_assert_int_eq(m2.val[1][1], 4); + ck_assert_int_eq(m2.val[1][2], 0); + ck_assert_int_eq(m2.val[2][0], 0); + ck_assert_int_eq(m2.val[2][1], 0); + ck_assert_int_eq(m2.val[2][2], 1); + + matrix_mult_vec(&m2, &x, &y); + ck_assert_int_eq(x, 200); + ck_assert_int_eq(y, 20); + + matrix_init_translate(&m2, 10, 100); + ck_assert_int_eq(m2.val[0][0], 1); + ck_assert_int_eq(m2.val[0][1], 0); + ck_assert_int_eq(m2.val[0][2], 10); + ck_assert_int_eq(m2.val[1][0], 0); + ck_assert_int_eq(m2.val[1][1], 1); + ck_assert_int_eq(m2.val[1][2], 100); + ck_assert_int_eq(m2.val[2][0], 0); + ck_assert_int_eq(m2.val[2][1], 0); + ck_assert_int_eq(m2.val[2][2], 1); + + matrix_mult_vec(&m2, &x, &y); + ck_assert_int_eq(x, 210); + ck_assert_int_eq(y, 120); + + matrix_to_farray6(&m2, f); + ck_assert_int_eq(f[0], 1); + ck_assert_int_eq(f[1], 0); + ck_assert_int_eq(f[2], 10); + ck_assert_int_eq(f[3], 0); + ck_assert_int_eq(f[4], 1); + ck_assert_int_eq(f[5], 100); +} +END_TEST + int main (int argc, char **argv) { litest_add_no_device("events:conversion", event_conversion_device_notify); litest_add_no_device("events:conversion", event_conversion_pointer); @@ -441,5 +518,6 @@ int main (int argc, char **argv) { litest_add("device:id", device_ids, LITEST_ANY, LITEST_ANY); litest_add_no_device("config:status string", config_status_string); + litest_add_no_device("misc:matrix", matrix_helpers); return litest_run(argc, argv); } -- 2.34.1