line circle and polygon circle.

FoamyGuy · FoamyGuy · commit b0fccc8b90e8 · 2024-09-21T12:01:21.000-05:00
diff --git 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,37 +324,115 @@ 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)];
     mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
 
 	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
@@ -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
@@ -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,20 +174,26 @@ 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;
 		int next_index = (i + 1) % len;
         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
@@ -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);
