algorithm/line_vacuate/studio_geo_algo_c.c

/**
 ******************************************************************************
 * @file           : studio_geo_algo_c.c
 * @author         : wangyingjie
 * @brief          : None
 * @attention      : None
 * @date           : 2025/5/11
 ******************************************************************************
 */

#include "studio_geo_algo_c.h"
#include "geography/studio_proj_c.h"

/////////////////// 基础适量算法 ///////////////////

double distance_squared_c(const studio_point_c *p1, const studio_point_c *p2)
{
    const double dx = p1->x - p2->x;
    const double dy = p1->y - p2->y;
    return dx * dx + dy * dy;
}

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;
}

double azimuth_angle_c(const studio_point_c *p_from, const studio_point_c *p_to)
{
    double delta_x = p_to->x - p_from->x;
    double delta_y = p_to->y - p_from->y;

    double theta_rad = atan2(delta_x, delta_y);
    double theta_deg = theta_rad * (180.0 / AO_M_PI);

    if (theta_deg < 0.0)
    {
        theta_deg += 360.0;
    }
    return theta_deg;
}

double distance_squared_gcs_c(const studio_point_c *p1, const studio_point_c *p2)
{
    // 经纬度转高斯投影
    // double central = ((int) (p1->x) / 3) * 3;
    double central = floor((p1->x + 1.5) / 3.0) * 3.0;
    double gx_1, gy_1;
    lonlat_to_gauss(central, p1->x, p1->y, &gx_1, &gy_1);
    studio_point_c gs_p1 = studio_point_init(gx_1, gy_1);
    double gx_2, gy_2;
    lonlat_to_gauss(central, p2->x, p2->y, &gx_2, &gy_2);
    studio_point_c gs_p2 = studio_point_init(gx_2, gy_2);
    double dist_square = distance_squared_c(&gs_p1, &gs_p2);
    return dist_square;
}

double point_line_dist_square_gcs_c(const studio_point_c *p, const studio_point_c *start, const studio_point_c *end)
{
    // 经纬度转高斯投影
    // double central = ((int) (p->x) / 3) * 3;
    double central = floor((p->x + 1.5) / 3.0) * 3.0;
    double gx_p, gy_p;
    lonlat_to_gauss(central, p->x, p->y, &gx_p, &gy_p);
    studio_point_c gs_p = studio_point_init(gx_p, gy_p);
    double gx_start, gy_start;
    lonlat_to_gauss(central, start->x, start->y, &gx_start, &gy_start);
    studio_point_c gs_start = studio_point_init(gx_start, gy_start);
    double gx_end, gy_end;
    lonlat_to_gauss(central, end->x, end->y, &gx_end, &gy_end);
    studio_point_c gs_end = studio_point_init(gx_end, gy_end);
    double dist_square = point_line_dist_square_c(&gs_p, &gs_start, &gs_end);
    return dist_square;
}

double azimuth_angle_gcs_c(const studio_point_c *p_from, const studio_point_c *p_to)
{
    // 经纬度转高斯投影
    // double central = ((int) (p_from->x) / 3) * 3;
    double central = floor((p_from->x + 1.5) / 3.0) * 3.0;
    double gx_from, gy_from;
    lonlat_to_gauss(central, p_from->x, p_from->y, &gx_from, &gy_from);
    studio_point_c gs_from = studio_point_init(gx_from, gy_from);
    double gx_to, gy_to;
    lonlat_to_gauss(central, p_to->x, p_to->y, &gx_to, &gy_to);
    studio_point_c gs_to = studio_point_init(gx_to, gy_to);
    double theta_deg = azimuth_angle_c(&gs_from, &gs_to);
    return theta_deg;
}

studio_point_c lonlat_move_by(const studio_point_c *pt, double delta_x, double delta_y)
{
    // 中央经线
    double central = floor((pt->x + 1.5) / 3.0) * 3.0;

    // 转换为高斯平面坐标
    double gx, gy;
    lonlat_to_gauss(central, pt->x, pt->y, &gx, &gy);

    // 应用平面坐标偏移（东向加delta_x，北向加delta_y）
    const double new_gx = gx + delta_x;  // 单位：米
    const double new_gy = gy + delta_y;

    // 转回经纬度
    double new_lon, new_lat;
    gauss_to_lonlat(central, new_gx, new_gy, &new_lon, &new_lat);

    // 经度范围修正（-180~180）
    new_lon = fmod(new_lon + 360.0, 360.0);  // 先转0~360
    if (new_lon > 180.0)
    {
        new_lon -= 360.0;  // 再转-180~180
    }
    studio_point_c new_point = studio_point_init(new_lon, new_lat);

    return new_point;
}

/////////////////// 矢量(线)抽稀算法 ///////////////////

void douglas_peucker_c(const studio_line_c *points, size_t start, size_t end, double epsilon, unsigned int *indices, unsigned int *index_count, unsigned int max_indices)
{
    if (end <= start + 1)
    {
        if (*index_count < max_indices)
        {
            indices[*index_count] = start;
            (*index_count)++;
        }
        if (start != end && *index_count < max_indices)
        {
            indices[*index_count] = end;
            (*index_count)++;
        }
        return;
    }

    double max_dist = 0;
    size_t index = start;
    double epsilon_square = epsilon * epsilon;

    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;
        }
    }

    if (max_dist > epsilon_square && index > start && index < end)
    {
        douglas_peucker_c(points, start, index, epsilon, indices, index_count, max_indices);
        douglas_peucker_c(points, index, end, epsilon, indices, index_count, max_indices);
    }
    else
    {
        if (*index_count < max_indices)
        {
            indices[*index_count] = start;
            (*index_count)++;
        }
        if (*index_count < max_indices && start != end)
        {
            indices[*index_count] = end;
            (*index_count)++;
        }
    }
}

void remove_duplicates(unsigned int *indices, unsigned int *index_count)
{
    if (*index_count == 0)
    {
        return;
    }
    unsigned int j = 0;  // 指向去重后的位置
    for (unsigned int i = 0; i < *index_count; i++)
    {
        bool is_duplicate = false;

        // 检查 indices[i] 是否已经在 indices[0..j-1] 中出现过
        for (unsigned int k = 0; k < j; k++)
        {
            if (indices[i] == indices[k])
            {
                is_duplicate = true;
                break;
            }
        }

        // 如果不是重复元素，则加入去重后的数组
        if (!is_duplicate)
        {
            indices[j++] = indices[i];
        }
    }

    *index_count = j;  // 更新数组长度
}

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));
    if (!indices)
    {
        // 内存分配失败处理
        return false;
    }
    unsigned int index_count = 0;
    const unsigned int max_indices = line->size;

    double t_epsilon = epsilon;

    // 循环执行Douglas-Peucker算法，直到索引点数小于等于最大点数
    while (1)
    {
        index_count = 0;
        // douglas_peucker_c(line, 0, line->size - 1, t_epsilon, indices, &index_count);
        douglas_peucker_c(line, 0, line->size - 1, t_epsilon, indices, &index_count, max_indices);
        // 将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;
}

bool line_vacuate_gcs_c(const studio_line_c *line, const int max_points, const double epsilon, studio_line_c *vacuate_line)
{
    bool status = false;
    if (!line || !vacuate_line || line->size <= 1)  // 添加空指针和长度校验
    {
        printf("line or vacuate_line is null or line size <= 1");
        return status;
    }
    // 经纬度转高斯克吕格投影
    studio_line_c gauss_line = studio_line_c_init();
    // double central = ((int) (line->data[0].x) / 3) * 3;
    double central = floor((line->data[0].x + 1.5) / 3.0) * 3.0;
    for (unsigned int i = 0; i < line->size; i++)
    {
        double gx, gy;
        lonlat_to_gauss(central, line->data[i].x, line->data[i].y, &gx, &gy);
        studio_point_c point = studio_point_init(gx, gy);
        studio_line_c_add_point(&gauss_line, point);
    }
    studio_line_c vac_gauss_line = studio_line_c_init();
    bool res = line_vacuate_c(&gauss_line, max_points, epsilon, &vac_gauss_line);
    if (!res)
    {
        printf("Failed to simplify line.");
        return status;
    }
    studio_line_c_destroy(&gauss_line);
    // 高斯克吕格转经纬度
    for (unsigned int i = 0; i < vac_gauss_line.size; i++)
    {
        double lon, lat;
        gauss_to_lonlat(central, vac_gauss_line.data[i].x, vac_gauss_line.data[i].y, &lon, &lat);
        studio_point_c point = studio_point_init(lon, lat);
        studio_line_c_add_point(vacuate_line, point);
    }
    studio_line_c_destroy(&vac_gauss_line);
    status = true;
    return status;
}

bool remove_outliers_c(const studio_line_c *line, double distance, studio_line_c *outliers_line)
{
    bool status = false;
    if (!line || !outliers_line || line->size <= 1)  // 添加空指针和长度校验
    {
        printf("line or outliers_line is null or line size <= 1");
        return status;
    }
    double dist_square = distance * distance;
    // 经纬度转高斯克吕格投影
    studio_line_c gauss_line = studio_line_c_init();
    // double central = ((int) (line->data[0].x) / 3) * 3;
    double central = floor((line->data[0].x + 1.5) / 3.0) * 3.0;
    for (unsigned int i = 0; i < line->size; i++)
    {
        double gx, gy;
        lonlat_to_gauss(central, line->data[i].x, line->data[i].y, &gx, &gy);
        studio_point_c point = studio_point_init(gx, gy);
        studio_line_c_add_point(&gauss_line, point);
    }

    // 离群点检测 ------------------------------------------------
    if (gauss_line.size <= 1)
    {
        printf("gauss_line size <= 1");
        return status;
    }
    studio_line_c gs_filtered_line = studio_line_c_init();           // 新建过滤后的线段
    studio_line_c_add_point(&gs_filtered_line, gauss_line.data[0]);  // 保留第一个点
    for (unsigned int i = 1; i < gauss_line.size; i++)               // 遍历相邻点对
    {
        double d_sq = distance_squared_c(&gauss_line.data[i - 1], &gauss_line.data[i]);
        if (d_sq <= dist_square)
        {
            studio_line_c_add_point(&gs_filtered_line, gauss_line.data[i]);
        }
        else
        {
            int a = 0;
        }
    }
    studio_line_c_destroy(&gauss_line);  // 释放原始数据的高斯投影

    // 高斯克吕格转经纬度
    for (unsigned int i = 0; i < gs_filtered_line.size; i++)
    {
        double lon, lat;
        gauss_to_lonlat(central, gs_filtered_line.data[i].x, gs_filtered_line.data[i].y, &lon, &lat);
        studio_point_c point = studio_point_init(lon, lat);
        studio_line_c_add_point(outliers_line, point);
    }

    studio_line_c_destroy(&gs_filtered_line);  // 释放移除离群点后的高斯投影

    status = true;
    return status;
}