first commit

vertix.cpp

@@ -0,0 +1,137 @@
+#include "vertix.h"
+#include <stdlib.h>
+#include "pose_body.h"
+
+Polygon workspace;
+
+void dump_polygon_to_file(char *filename) {
+    FILE *fp = fopen(filename, "w+");
+    if (fp == NULL)
+    {
+        printf("failed to open fp\n");
+        return;
+    }
+
+    char s[100];
+    snprintf(s, 100, "rect: [");
+    fwrite(s, strlen(s), 1, fp);
+    for (int i = 0; i < workspace.numVertix; i++)
+    {
+        if (i != workspace.numVertix - 1)
+        {
+            snprintf(s, 100, "(%f, %f), ", workspace.vertix[i].x, workspace.vertix[i].y);
+        }
+        else
+        {
+            snprintf(s, 100, "(%f, %f)", workspace.vertix[i].x, workspace.vertix[i].y);
+        }
+        fwrite(s, strlen(s), 1, fp);
+    }
+    const char *suffix = "]\n";
+    fwrite(suffix, strlen(suffix), 1, fp);
+    fclose(fp);
+
+}
+
+void init_polygon(float *datas, int point_size)
+{
+    if (point_size == 0)
+    {
+        workspace.numVertix = 0;
+#ifdef DUMP_POLYGON_INFO
+        char filename[128];
+        snprintf(filename, 128, "/usr/data/camera/%s/dump_rect.txt", CameraID);
+        dump_polygon_to_file(filename);
+#endif
+        return;
+    }
+    if (point_size % 2 != 0)
+    {
+        printf("point size error");
+        exit(0);
+    }
+    workspace.numVertix = point_size / 2;
+    workspace.vertix = (Point *)malloc(sizeof(Point) * point_size);
+    for (int i = 0; i < point_size / 2; i++)
+    {
+        workspace.vertix[i] = Point{
+            x : datas[i * 2],
+            y : datas[i * 2 + 1],
+        };
+    }
+
+#ifdef DUMP_POLYGON_INFO
+    char filename[128];
+    snprintf(filename, 128, "/usr/data/camera/%s/dump_rect.txt", CameraID);
+    dump_polygon_to_file(filename);
+#endif
+}
+
+void dump_box_info(bool is_in, int left, int top, int right, int bottom)
+{
+    char filename[128];
+    snprintf(filename, 128, "/usr/data/camera/%s/dump_rect.txt", CameraID);
+    FILE *fp = fopen(filename, "a+");
+    if (fp == NULL)
+    {
+        printf("failed to open fp\n");
+        return;
+    }
+
+    float y = (top + bottom) / 2.0;
+    float x = (left + right) / 2.0;
+    char s[] = {0};
+    if (is_in)
+    {
+        snprintf(s, 1024, "in: [%d, %d, %d, %d](%f, %f)\n",
+                 left, top, right, bottom, x, y);
+    }
+    else
+    {
+        snprintf(s, 1024, "not in: [%d, %d, %d, %d](%f, %f)\n",
+                 left, top, right, bottom, x, y);
+    }
+    fwrite(s, strlen(s), 1, fp);
+    fclose(fp);
+}
+
+bool is_point_in_workspace(int left, int top, int right, int bottom)
+{
+
+    float y = (top + bottom) / 2.0;
+    float x = (left + right) / 2.0;
+    int n = workspace.numVertix;
+    if (n == 0)
+    {
+        // all area
+#ifdef DUMP_POLYGON_INFO
+        dump_box_info(true, left, top, right, bottom);
+#endif
+        printf("all workspace\n");
+        return true;
+    }
+
+    int i, j;
+    bool inside = false;
+
+    for (i = 0, j = n - 1; i < n; j = i++)
+    {
+        if ((workspace.vertix[i].y > y) != (workspace.vertix[j].y > y))
+        {
+            // this point is between the Point[j] and Point[i] in y axis
+            float deltax = (workspace.vertix[j].x - workspace.vertix[i].x);
+            float deltay = (workspace.vertix[j].y - workspace.vertix[i].y);
+            float ratio = deltax / deltay;
+
+            if (x < (ratio * (y - workspace.vertix[i].y) + workspace.vertix[i].x))
+            {
+                inside = !inside;
+            }
+        }
+    }
+
+#ifdef DUMP_POLYGON_INFO
+    dump_box_info(inside, left, top, right, bottom);
+#endif
+    return inside;
+}