137 lines
3.4 KiB
C++
137 lines
3.4 KiB
C++
#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;
|
|
} |