|
@@ -0,0 +1,168 @@
|
|
|
+/**
|
|
|
+ ******************************************************************************
|
|
|
+ * @file : studio_geo_algo_c.c
|
|
|
+ * @author : wangyingjie
|
|
|
+ * @brief : None
|
|
|
+ * @attention : None
|
|
|
+ * @date : 2025/5/11
|
|
|
+ ******************************************************************************
|
|
|
+ */
|
|
|
+
|
|
|
+#include "studio_geo_algo_c.h"
|
|
|
+
|
|
|
+
|
|
|
+double point_line_dist_square_c(const studio_point_c *p, const studio_point_c *start, const studio_point_c *end)
|
|
|
+{
|
|
|
+ double A = p->x - start->x;
|
|
|
+ double B = p->y - start->y;
|
|
|
+ double C = end->x - start->x;
|
|
|
+ double D = end->y - start->y;
|
|
|
+
|
|
|
+ double dot = A * C + B * D;
|
|
|
+ double len_sq = C * C + D * D;
|
|
|
+ double param = len_sq == 0 ? -1 : dot / len_sq;
|
|
|
+
|
|
|
+ double xx, yy;
|
|
|
+
|
|
|
+ if (param < 0)
|
|
|
+ {
|
|
|
+ xx = start->x;
|
|
|
+ yy = start->y;
|
|
|
+ }
|
|
|
+ else if (param > 1)
|
|
|
+ {
|
|
|
+ xx = end->x;
|
|
|
+ yy = end->y;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ xx = start->x + param * C;
|
|
|
+ yy = start->y + param * D;
|
|
|
+ }
|
|
|
+
|
|
|
+ double dist_square = (xx - p->x) * (xx - p->x) + (yy - p->y) * (yy - p->y);
|
|
|
+ return dist_square;
|
|
|
+}
|
|
|
+
|
|
|
+// Douglas-Peucker算法,用于简化多边形或曲线
|
|
|
+void douglas_peucker_c(const studio_line_c *points, size_t start, size_t end, double epsilon, unsigned int *indices, unsigned int *index_count)
|
|
|
+{
|
|
|
+ // 计算最大距离
|
|
|
+ double max_dist = 0;
|
|
|
+ size_t index = start;
|
|
|
+ double epsilon_square = epsilon * epsilon;
|
|
|
+
|
|
|
+ // 遍历从start+1到end-1的点
|
|
|
+ for (size_t i = start + 1; i < end; ++i)
|
|
|
+ {
|
|
|
+ // 计算点到线段的距离的平方
|
|
|
+ double dist_square = point_line_dist_square_c(&points->data[i], &points->data[start], &points->data[end]);
|
|
|
+ // 如果距离大于最大距离,则更新最大距离和索引
|
|
|
+ if (dist_square > max_dist)
|
|
|
+ {
|
|
|
+ index = i;
|
|
|
+ max_dist = dist_square;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // 如果最大距离大于epsilon的平方,并且索引不是start和end,则递归调用Douglas-Peucker算法
|
|
|
+ if (max_dist > epsilon_square && index != start && index != end)
|
|
|
+ {
|
|
|
+ douglas_peucker_c(points, start, index, epsilon, indices, index_count);
|
|
|
+ douglas_peucker_c(points, index, end, epsilon, indices, index_count);
|
|
|
+ }
|
|
|
+ // 否则,将start和end的索引添加到indices数组中
|
|
|
+ else
|
|
|
+ {
|
|
|
+ indices[*index_count] = start;
|
|
|
+ (*index_count)++;
|
|
|
+ if (start != end)
|
|
|
+ {
|
|
|
+ indices[*index_count] = end;
|
|
|
+ (*index_count)++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void remove_duplicates(unsigned int **indices, unsigned int *size) {
|
|
|
+ if (*size == 0) return;
|
|
|
+ // 临时数组用于存储去重后的数据
|
|
|
+ unsigned int *unique_indices = (unsigned int *)malloc(*size * sizeof(unsigned int));
|
|
|
+ unsigned int unique_count = 0;
|
|
|
+ for (unsigned int i = 0; i < *size; i++) {
|
|
|
+ unsigned int is_duplicate = 0;
|
|
|
+ // 检查当前元素是否已经在 unique_indices 中
|
|
|
+ for (unsigned int j = 0; j < unique_count; j++) {
|
|
|
+ if ((*indices)[i] == unique_indices[j]) {
|
|
|
+ is_duplicate = 1;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // 如果没有重复,添加到 unique_indices
|
|
|
+ if (!is_duplicate) {
|
|
|
+ unique_indices[unique_count] = (*indices)[i];
|
|
|
+ unique_count++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // 释放原有的 indices
|
|
|
+ free(*indices);
|
|
|
+ // 更新 indices 指针和大小
|
|
|
+ *indices = unique_indices;
|
|
|
+ *size = unique_count;
|
|
|
+}
|
|
|
+
|
|
|
+bool line_vacuate_c(const studio_line_c *line, const int max_points, const double epsilon, studio_line_c *vacuate_line)
|
|
|
+{
|
|
|
+ // 初始化状态变量
|
|
|
+ bool status = false;
|
|
|
+
|
|
|
+ // 如果输入的线段为空,则打印错误信息并返回false
|
|
|
+ if (line->size == 0)
|
|
|
+ {
|
|
|
+ printf("line is empty\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 如果输入的线段点数小于等于最大点数,则直接将线段点添加到vacuate_line中,并返回true
|
|
|
+ if (line->size <= max_points)
|
|
|
+ {
|
|
|
+ for (size_t i = 0; i < line->size; ++i)
|
|
|
+ {
|
|
|
+ studio_line_c_add_point(vacuate_line, line->data[i]);
|
|
|
+ }
|
|
|
+ status = true;
|
|
|
+ return status;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 分配内存空间存储线段点的索引
|
|
|
+ unsigned int *indices = (unsigned int *)malloc(line->size * sizeof(unsigned int));
|
|
|
+ unsigned int index_count = 0;
|
|
|
+
|
|
|
+ double t_epsilon = epsilon;
|
|
|
+
|
|
|
+ // 循环执行Douglas-Peucker算法,直到索引点数小于等于最大点数
|
|
|
+ while (1)
|
|
|
+ {
|
|
|
+ index_count = 0;
|
|
|
+ douglas_peucker_c(line, 0, line->size - 1, t_epsilon, indices, &index_count);
|
|
|
+ // 将indices 中的数据去重
|
|
|
+ remove_duplicates(&indices, &index_count);
|
|
|
+
|
|
|
+ if (index_count <= (unsigned int)max_points)
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ t_epsilon *= 1.1;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 将索引点添加到vacuate_line中
|
|
|
+ for (unsigned int i = 0; i < index_count; ++i)
|
|
|
+ {
|
|
|
+ studio_line_c_add_point(vacuate_line, line->data[indices[i]]);
|
|
|
+ }
|
|
|
+
|
|
|
+ // 释放内存空间
|
|
|
+ free(indices);
|
|
|
+ status = true;
|
|
|
+ return status;
|
|
|
+}
|