starting polygon circle.

FoamyGuy · Sep 14, 2024 · a1ac54b · a1ac54b
1 parent 5db420a
commit a1ac54b
Show file tree

Hide file tree

Showing 4 changed files with 294 additions and 0 deletions.
diff --git a/shared-bindings/vectorio/__init__.c b/shared-bindings/vectorio/__init__.c
@@ -185,10 +185,193 @@ static mp_obj_t vectorio_circle_circle_intersects(size_t n_args, const mp_obj_t
 }
 MP_DEFINE_CONST_FUN_OBJ_KW(vectorio_circle_circle_intersects_obj, 0, vectorio_circle_circle_intersects);
 
+//| def circle_contains_point(
+//|     cx: int, cy: int, cr: int, px: int, py: int
+//| ) -> bool:
+//|     """Checks whether a circle contains the given point
+//|
+//|     :param int cx: Circle center x coordinate
+//|     :param int cy: Circle center y coordinate
+//|     :param int cr: Circle radius
+//|     :param int px: Point x coordinate
+//|     :param int py: Point y coordinate
+//|     ...
+//|
+static mp_obj_t vectorio_circle_contains_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    enum {ARG_cx, ARG_cy, ARG_cr, ARG_px, ARG_py};
+
+    static const mp_arg_t allowed_args[] = {
+        {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_px, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+        {MP_QSTR_py, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+    };
+
+    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 cx = args[ARG_cx].u_int;
+    int16_t cy = args[ARG_cy].u_int;
+    int16_t cr = args[ARG_cr].u_int;
+    int16_t px = args[ARG_px].u_int;
+    int16_t py = args[ARG_py].u_int;
+
+    bool result = common_hal_vectorio_circle_contains_point(cx, cy, cr, px, py);
+    if (result) {
+        return mp_const_true;
+    } else {
+        return mp_const_false;
+    }
+}
+MP_DEFINE_CONST_FUN_OBJ_KW(vectorio_circle_contains_point_obj, 0, vectorio_circle_contains_point);
+
+//| def rectangle_contains_point(
+//|     rx: int, ry: int, rw: int, rh: int, px: int, py: int
+//| ) -> bool:
+//|     """Checks whether a rectangle contains the given point
+//|
+//|     :param int rx: Rectangle x coordinate
+//|     :param int ry: Rectangle y coordinate
+//|     :param int rw: Rectangle width
+//|     :param int rh: Rectangle height
+//|     :param int px: Point x coordinate
+//|     :param int py: Point y coordinate
+//|     ...
+//|
+static mp_obj_t vectorio_rectangle_contains_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    enum {ARG_rx, ARG_ry, ARG_rw, ARG_rh, ARG_px, ARG_py};
+
+    static const mp_arg_t allowed_args[] = {
+        {MP_QSTR_rx, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+        {MP_QSTR_ry, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+        {MP_QSTR_rw, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+        {MP_QSTR_rh, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+        {MP_QSTR_px, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+        {MP_QSTR_py, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+    };
+
+    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 rx = args[ARG_rx].u_int;
+    int16_t ry = args[ARG_ry].u_int;
+    int16_t rw = args[ARG_rw].u_int;
+    int16_t rh = args[ARG_rh].u_int;
+    int16_t px = args[ARG_px].u_int;
+    int16_t py = args[ARG_py].u_int;
+
+    bool result = common_hal_vectorio_rectangle_contains_point(rx, ry, rw, rh, px, py);
+    if (result) {
+        return mp_const_true;
+    } else {
+        return mp_const_false;
+    }
+}
+MP_DEFINE_CONST_FUN_OBJ_KW(vectorio_rectangle_contains_point_obj, 0, vectorio_rectangle_contains_point);
+
+
+//| def line_contains_point(
+//|     x1: int, y1: int, x2: int, y2: int, px: int, py: int
+//| ) -> bool:
+//|     """Checks whether a line contains the given point
+//|
+//|     :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 px: Point x coordinate
+//|     :param int py: Point y coordinate
+//|     :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) {
+    enum {ARG_x1, ARG_y1, ARG_x2, ARG_y2, ARG_px, ARG_py, 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_px, MP_ARG_REQUIRED | MP_ARG_INT, {.u_obj = MP_OBJ_NULL}},
+        {MP_QSTR_py, 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 px = args[ARG_px].u_int;
+    int16_t py = args[ARG_py].u_int;
+
+    // Confirm the angle value
+    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_contains_point(x1, y1, x2, y2, px, py, padding);
+    if (result) {
+        return mp_const_true;
+    } else {
+        return mp_const_false;
+    }
+}
+MP_DEFINE_CONST_FUN_OBJ_KW(vectorio_line_contains_point_obj, 0, vectorio_line_contains_point);
+
+
+//| def polygon_circle_intersects(
+//|     points: List[Tuple[int, int]], cx: int, cy: int, cr: int
+//| ) -> bool:
+//|     """Checks for intersection between a polygon and a cricle.
+//|
+//|     :param List[Tuple[int,int]] points: Vertices for the polygon
+//|     :param int cx: Circle center x coordinate
+//|     :param int cy: Circle center y coordinate
+//|     :param int cr: Circle radius"""
+//|     ...
+//|
+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};
+
+    static const mp_arg_t allowed_args[] = {
+		{MP_QSTR_points, MP_ARG_REQUIRED | MP_ARG_OBJ, {.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_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 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);
+    if (result) {
+        return mp_const_true;
+    } else {
+        return mp_const_false;
+    }
+}
+MP_DEFINE_CONST_FUN_OBJ_KW(vectorio_polygon_circle_intersects_obj, 0, vectorio_polygon_circle_intersects);
+
+
 static const mp_rom_map_elem_t vectorio_module_globals_table[] = {
     { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_vectorio) },
     { 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_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) },
     { MP_ROM_QSTR(MP_QSTR_rectangle_rectangle_intersects), MP_ROM_PTR(&vectorio_rectangle_rectangle_intersects_obj) },
     { MP_ROM_QSTR(MP_QSTR_Circle), MP_ROM_PTR(&vectorio_circle_type) },
     { MP_ROM_QSTR(MP_QSTR_Polygon), MP_ROM_PTR(&vectorio_polygon_type) },

diff --git a/shared-bindings/vectorio/__init__.h b/shared-bindings/vectorio/__init__.h
@@ -56,3 +56,18 @@ bool common_hal_vectorio_rectangle_rectangle_intersects(
 bool common_hal_vectorio_circle_circle_intersects(
     int16_t c1x, int16_t c1y, int16_t c1r,
     int16_t c2x, int16_t c2y, int16_t c2r);
+
+bool common_hal_vectorio_circle_contains_point(
+    int16_t cx, int16_t cy, int16_t cr,
+    int16_t px, int16_t py);
+
+bool common_hal_vectorio_rectangle_contains_point(
+    int16_t rx, int16_t ry, int16_t rw, int16_t rh,
+    int16_t px, int16_t py);
+
+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_polygon_circle_intersects(
+	mp_obj_t points_list, int16_t cx, int16_t cy, int16_t cr);
diff --git a/shared-module/vectorio/__init__.c b/shared-module/vectorio/__init__.c
@@ -7,6 +7,8 @@
 // Don't need anything in here yet
 
 #include "shared-bindings/vectorio/__init__.h"
+#include "shared-module/vectorio/__init__.h"
+
 #include "py/runtime.h"
 #include "stdlib.h"
 #include <math.h>
@@ -56,6 +58,63 @@ bool common_hal_vectorio_circle_circle_intersects(
 
 }
 
+bool common_hal_vectorio_circle_contains_point(
+    int16_t cx, int16_t cy, int16_t cr,
+    int16_t px, int16_t py
+    ) {
+
+    mp_int_t dist_x = px - cx;
+    mp_int_t dist_y = py - cy;
+
+	mp_int_t dist_sq = (dist_x * dist_x) + (dist_y * dist_y);
+
+    return dist_sq <= cr * cr;
+}
+
+bool common_hal_vectorio_rectangle_contains_point(
+    int16_t rx, int16_t ry, int16_t rw, int16_t rh,
+    int16_t px, int16_t py
+    ) {
+
+	if (rx > px){
+		return false;
+	}
+	if (px > rx + rw){
+		return false;
+	}
+	if (ry > py){
+		return false;
+	}
+	if (py > ry + rh){
+		return false;
+	}
+    return true;
+}
+
+float measure_distance(int x1, int y1, int x2, int y2){
+	int dist_x = x1 - x2;
+	int dist_y = y1 - y2;
+	return sqrtf((dist_x * dist_x) + (dist_y * dist_y));
+}
+
+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
+    ) {
+
+	float line_length = measure_distance(x1, y1, x2, y2);
+	float d1 = measure_distance(x1, y1, px, py);
+	float d2 = measure_distance(x2, y2, px, py);
+
+	if (d1+d2 >= line_length-padding && d1+d2 <= line_length+padding) {
+    	return true;
+	}
+	return false;
+
+}
+
+
+
 bool common_hal_vectorio_rectangle_rectangle_intersects(
     int16_t r1x, int16_t r1y, int16_t r1w, int16_t r1h,
     int16_t r2x, int16_t r2y, int16_t r2w, int16_t r2h) {
@@ -74,3 +133,37 @@ bool common_hal_vectorio_rectangle_rectangle_intersects(
     return r1_left < r2_right && r1_right > r2_left &&
            r1_top < r2_bottom && r1_bottom > r2_top;
 }
+
+bool common_hal_vectorio_polygon_circle_intersects(
+	mp_obj_t points_list, int16_t cx, int16_t cy, int16_t cr
+) {
+	size_t len = 0;
+	mp_obj_t *points_list_items;
+    mp_obj_list_get(points_list, &len, &points_list_items);
+
+	for(uint16_t i = 0; i < len; i++){
+		size_t cur_tuple_len = 0;
+		mp_obj_t *cur_point_tuple;
+        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);
+
+		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_printf(&mp_plat_print, "%d,%d - %d,%d \n", cur_x, cur_y, next_x, next_y);
+
+
+
+	}
+
+	return false;
+}
diff --git a/shared-module/vectorio/__init__.h b/shared-module/vectorio/__init__.h
@@ -14,3 +14,6 @@ typedef struct {
     mp_obj_t obj;
     event_function *event;
 } vectorio_event_t;
+
+float measure_distance(
+    int x1, int y1, int x2, int y2);