diff --git a/shared-bindings/vectorio/__init__.c b/shared-bindings/vectorio/__init__.c index 08a6711fcbec..7f61de75dddf 100644 --- a/shared-bindings/vectorio/__init__.c +++ b/shared-bindings/vectorio/__init__.c @@ -282,7 +282,7 @@ MP_DEFINE_CONST_FUN_OBJ_KW(vectorio_rectangle_contains_point_obj, 0, vectorio_re //| :param int y2: Line y2 coordinate //| :param int px: Point x coordinate //| :param int py: Point y coordinate -//| :param float padding: Extra padding outside of the line to consider as positive intersection +//| :param float padding: Extra padding outside of the line to consider as positive intersection""" //| ... //| static mp_obj_t vectorio_line_contains_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -308,7 +308,7 @@ static mp_obj_t vectorio_line_contains_point(size_t n_args, const mp_obj_t *pos_ int16_t px = args[ARG_px].u_int; int16_t py = args[ARG_py].u_int; - // Confirm the angle value + // Use default padding if None was passed mp_float_t padding = 0.0; if (args[ARG_padding].u_obj != mp_const_none) { padding = mp_obj_get_float(args[ARG_padding].u_obj); @@ -324,25 +324,90 @@ static mp_obj_t vectorio_line_contains_point(size_t n_args, const mp_obj_t *pos_ MP_DEFINE_CONST_FUN_OBJ_KW(vectorio_line_contains_point_obj, 0, vectorio_line_contains_point); +//| def line_circle_intersects( +//| x1: int, y1: int, x2: int, y2: int, +// cx: int, cy: int, cr: int +//| ) -> bool: +//| """Checks whether a line intersects with a circle +//| +//| :param int x1: Line x1 coordinate +//| :param int y1: Line y1 coordinate +//| :param int x2: Line x2 coordinate +//| :param int y2: Line y2 coordinate +//| :param int cx: Circle center x coordinate +//| :param int cy: Circle center y coordinate +//| :param int cr: Circle radius +//| :param float padding: Extra padding outside of the line to consider as positive intersection +//| ... +//| +static mp_obj_t vectorio_line_circle_intersects(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum {ARG_x1, ARG_y1, ARG_x2, ARG_y2, ARG_cx, ARG_cy, ARG_cr, ARG_padding}; + + static const mp_arg_t allowed_args[] = { + {MP_QSTR_x1, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_y1, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_x2, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_y2, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_cx, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_cy, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_cr, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_padding, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} }, // None convert to 0.0 + }; + + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + int16_t x1 = args[ARG_x1].u_int; + int16_t y1 = args[ARG_y1].u_int; + int16_t x2 = args[ARG_x2].u_int; + int16_t y2 = args[ARG_y2].u_int; + int16_t cx = args[ARG_cx].u_int; + int16_t cy = args[ARG_cy].u_int; + int16_t cr = args[ARG_cr].u_int; + + // Use default padding if None was passed + mp_float_t padding = 0.0; + if (args[ARG_padding].u_obj != mp_const_none) { + padding = mp_obj_get_float(args[ARG_padding].u_obj); + } + + bool result = common_hal_vectorio_line_circle_intersects(x1, y1, x2, y2, cx, cy, cr, padding); + + if (result) { + return mp_const_true; + } else { + return mp_const_false; + } +} +MP_DEFINE_CONST_FUN_OBJ_KW(vectorio_line_circle_intersects_obj, 0, vectorio_line_circle_intersects); + + //| def polygon_circle_intersects( -//| points: List[Tuple[int, int]], cx: int, cy: int, cr: int +//| points: List[Tuple[int, int]], polygon_x: int, polygon_y: int, +//| cx: int, cy: int, cr: int, padding: float //| ) -> bool: //| """Checks for intersection between a polygon and a cricle. //| //| :param List[Tuple[int,int]] points: Vertices for the polygon +//| :param int polygon_x: Polygon x coordinate. All other polygon points are relative to this +//| :param int polygon_y: Polygon y coordinate. All other polygon points are relative to this //| :param int cx: Circle center x coordinate //| :param int cy: Circle center y coordinate -//| :param int cr: Circle radius""" +//| :param int cr: Circle radius +//| :param float padding: Extra padding outside of the line to consider as positive intersection""" //| ... //| static mp_obj_t vectorio_polygon_circle_intersects(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - enum {ARG_points_list, ARG_cx, ARG_cy, ARG_cr}; + enum {ARG_points_list, ARG_polygon_x, ARG_polygon_y, ARG_cx, ARG_cy, ARG_cr, ARG_padding}; static const mp_arg_t allowed_args[] = { {MP_QSTR_points, MP_ARG_REQUIRED | MP_ARG_OBJ, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_polygon_x, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_polygon_y, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, {MP_QSTR_cx, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, {MP_QSTR_cy, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, - {MP_QSTR_cr, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}} + {MP_QSTR_cr, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}}, + {MP_QSTR_padding, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} }, // None convert to 0.0 }; mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; @@ -350,11 +415,24 @@ static mp_obj_t vectorio_polygon_circle_intersects(size_t n_args, const mp_obj_t mp_obj_t points_list = mp_arg_validate_type(args[ARG_points_list].u_obj, &mp_type_list, MP_QSTR_points); + int16_t polygon_x = args[ARG_polygon_x].u_int; + int16_t polygon_y = args[ARG_polygon_y].u_int; + int16_t cx = args[ARG_cx].u_int; int16_t cy = args[ARG_cy].u_int; int16_t cr = args[ARG_cr].u_int; - bool result = common_hal_vectorio_polygon_circle_intersects(points_list, cx, cy, cr); + // Use default padding if None was passed + mp_float_t padding = 0.0; + if (args[ARG_padding].u_obj != mp_const_none) { + padding = mp_obj_get_float(args[ARG_padding].u_obj); + } + + bool result = common_hal_vectorio_polygon_circle_intersects( + points_list, polygon_x, polygon_y, + cx, cy, cr, padding + ); + if (result) { return mp_const_true; } else { @@ -369,6 +447,7 @@ static const mp_rom_map_elem_t vectorio_module_globals_table[] = { { MP_ROM_QSTR(MP_QSTR_circle_rectangle_intersects), MP_ROM_PTR(&vectorio_circle_rectangle_intersects_obj) }, { MP_ROM_QSTR(MP_QSTR_polygon_circle_intersects), MP_ROM_PTR(&vectorio_polygon_circle_intersects_obj) }, { MP_ROM_QSTR(MP_QSTR_circle_circle_intersects), MP_ROM_PTR(&vectorio_circle_circle_intersects_obj) }, + { MP_ROM_QSTR(MP_QSTR_line_circle_intersects), MP_ROM_PTR(&vectorio_line_circle_intersects_obj) }, { MP_ROM_QSTR(MP_QSTR_circle_contains_point), MP_ROM_PTR(&vectorio_circle_contains_point_obj) }, { MP_ROM_QSTR(MP_QSTR_rectangle_contains_point), MP_ROM_PTR(&vectorio_rectangle_contains_point_obj) }, { MP_ROM_QSTR(MP_QSTR_line_contains_point), MP_ROM_PTR(&vectorio_line_contains_point_obj) }, diff --git a/shared-bindings/vectorio/__init__.h b/shared-bindings/vectorio/__init__.h index 75a22baf38e8..67f3d484c448 100644 --- a/shared-bindings/vectorio/__init__.h +++ b/shared-bindings/vectorio/__init__.h @@ -69,5 +69,10 @@ bool common_hal_vectorio_line_contains_point( int16_t x1, int16_t y1, int16_t x2, int16_t y2, int16_t px, int16_t py, mp_float_t padding); +bool common_hal_vectorio_line_circle_intersects( + int16_t x1, int16_t y1, int16_t x2, int16_t y2, + int16_t cx, int16_t cy, int16_t cr, mp_float_t padding); + bool common_hal_vectorio_polygon_circle_intersects( - mp_obj_t points_list, int16_t cx, int16_t cy, int16_t cr); + mp_obj_t points_list, int16_t polygon_x, int16_t polygon_y, + int16_t cx, int16_t cy, int16_t cr, mp_float_t padding); diff --git a/shared-module/vectorio/__init__.c b/shared-module/vectorio/__init__.c index 6feea2c1a6e5..a1c3bb12201b 100644 --- a/shared-module/vectorio/__init__.c +++ b/shared-module/vectorio/__init__.c @@ -91,9 +91,9 @@ bool common_hal_vectorio_rectangle_contains_point( return true; } -float measure_distance(int x1, int y1, int x2, int y2){ - int dist_x = x1 - x2; - int dist_y = y1 - y2; +float measure_distance(float x1, float y1, float x2, float y2){ + float dist_x = x1 - x2; + float dist_y = y1 - y2; return sqrtf((dist_x * dist_x) + (dist_y * dist_y)); } @@ -113,7 +113,34 @@ bool common_hal_vectorio_line_contains_point( } +bool common_hal_vectorio_line_circle_intersects( + int16_t x1, int16_t y1, int16_t x2, int16_t y2, + int16_t cx, int16_t cy, int16_t cr, mp_float_t padding +){ + if (common_hal_vectorio_circle_contains_point(cx, cy, cr, x1, y1)){ + return true; + } + if (common_hal_vectorio_circle_contains_point(cx, cy, cr, x2, y2)){ + return true; + } + float line_length = measure_distance(x1, y1, x2, y2); + float dot = ( ((cx-x1)*(x2-x1)) + ((cy-y1)*(y2-y1)) ) / pow(line_length,2); + + float closestX = x1 + (dot * (x2-x1)); + float closestY = y1 + (dot * (y2-y1)); + + if (!common_hal_vectorio_line_contains_point(x1,y1,x2,y2, closestX,closestY, padding)){ + return false; + } + float distance = measure_distance(closestX, closestY, cx, cy); + if (distance <= cr){ + return true; + }else{ + return false; + } + +} bool common_hal_vectorio_rectangle_rectangle_intersects( int16_t r1x, int16_t r1y, int16_t r1w, int16_t r1h, @@ -135,7 +162,7 @@ bool common_hal_vectorio_rectangle_rectangle_intersects( } bool common_hal_vectorio_polygon_circle_intersects( - mp_obj_t points_list, int16_t cx, int16_t cy, int16_t cr + mp_obj_t points_list, int16_t polygon_x, int16_t polygon_y, int16_t cx, int16_t cy, int16_t cr, mp_float_t padding ) { size_t len = 0; mp_obj_t *points_list_items; @@ -147,8 +174,8 @@ bool common_hal_vectorio_polygon_circle_intersects( mp_arg_validate_type(points_list_items[i], &mp_type_tuple, MP_QSTR_point); mp_obj_tuple_get(points_list_items[i], &cur_tuple_len, &cur_point_tuple); - mp_int_t cur_x = mp_arg_validate_type_int(cur_point_tuple[0], MP_QSTR_x); - mp_int_t cur_y = mp_arg_validate_type_int(cur_point_tuple[1], MP_QSTR_y); + mp_int_t cur_x = mp_arg_validate_type_int(cur_point_tuple[0], MP_QSTR_x) + polygon_x; + mp_int_t cur_y = mp_arg_validate_type_int(cur_point_tuple[1], MP_QSTR_y) + polygon_y; size_t next_tuple_len = 0; mp_obj_t *next_point_tuple; @@ -156,11 +183,17 @@ bool common_hal_vectorio_polygon_circle_intersects( mp_arg_validate_type(points_list_items[next_index], &mp_type_tuple, MP_QSTR_point); mp_obj_tuple_get(points_list_items[next_index], &next_tuple_len, &next_point_tuple); - mp_int_t next_x = mp_arg_validate_type_int(next_point_tuple[0], MP_QSTR_x); - mp_int_t next_y = mp_arg_validate_type_int(next_point_tuple[1], MP_QSTR_y); + mp_int_t next_x = mp_arg_validate_type_int(next_point_tuple[0], MP_QSTR_x) + polygon_x; + mp_int_t next_y = mp_arg_validate_type_int(next_point_tuple[1], MP_QSTR_y) + polygon_y; - mp_printf(&mp_plat_print, "%d,%d - %d,%d \n", cur_x, cur_y, next_x, next_y); + //mp_printf(&mp_plat_print, "%d,%d - %d,%d \n", cur_x, cur_y, next_x, next_y); + if(common_hal_vectorio_line_circle_intersects( + cur_x, cur_y, next_x, next_y, + cx, cy, cr, padding + )){ + return true; + } } diff --git a/shared-module/vectorio/__init__.h b/shared-module/vectorio/__init__.h index c49737274e4d..d61990e37c3a 100644 --- a/shared-module/vectorio/__init__.h +++ b/shared-module/vectorio/__init__.h @@ -16,4 +16,4 @@ typedef struct { } vectorio_event_t; float measure_distance( - int x1, int y1, int x2, int y2); + float x1, float y1, float x2, float y2);