toys/gps/docs/source/math/douglas.cpp

/****************************************************************************
 * Copyright (C) 2020 by Simulation Team, NavInfo Ltd.                      *
 *                                                                          *
 * This file is part of Simulation Core Software Stack.                     *
 *                                                                          *
 *   Simulation Core is a commerical autonomous driving simulation solution.*
 *                                                                          *
 *   You should have received a copy of the NavInfo EULA License along with *
 *   the software. If not, please contact us, <simulation@navinfo.com>.     *
 ****************************************************************************/

#include <vector>

#include "geo/private/math/douglas.h"
#include "geo/private/math/distance.h"
#include "geo/private/math/ns_geom_math.h"

namespace ns
{
namespace geo
{

/**
 * @brief 道格拉斯-扑克算法，原始型点不动,指定点是否被抽希掉放在valid中
 *
 * @param points [in]
 * @param ptNum
 * @param threshold
 * @param valid [out ]大小和ptNum相等,false表示当前点被抽希掉,true表示为采样后的有效点
 * @return true
 * @return false
 */
bool HadMath_douglasPeucker1(const PointD* points, uint32_t ptNum,
                             double threshold,         //
                             std::vector<bool>& valid) //
{
    if (points == NULL || ptNum <= 1 || threshold <= 0)
        return false;
    if (ptNum <= 2)
    {
        valid.insert(valid.begin(), ptNum, true);
        return true;
    }

    double dis = 0;
    PointD foot;

    uint32_t select = 1; // 初始值无用
    const PointD& start = points[0];
    const PointD& last = points[ptNum - 1];
    for (uint32_t i = 1; i < ptNum - 1; ++i)
    {
        double orthDis = pointSegmentDistance(points[i], start, last, foot);
        if (dis < orthDis)
        {
            select = i;
            dis = orthDis;
        }
    }

    if (dis < threshold)
    {
        valid.push_back(true);
        valid.insert(valid.begin() + 1, ptNum - 2, false);
        valid.push_back(true);
    }
    else // select一定在上面重新赋值
    {
        std::vector<bool> firstValid, secondValid;
        if (HadMath_douglasPeucker1(&start, select + 1, threshold, firstValid) &&
            HadMath_douglasPeucker1(&points[select], ptNum - select, threshold, secondValid))
        {
            valid.insert(valid.end(), firstValid.begin(), firstValid.end());
            valid.insert(valid.end(), secondValid.begin() + 1, secondValid.end());
        }
    }

    return true;
}

size_t douglasPeucker(PointD* points, size_t ptNum, double threshold, double zThreshold)
{
    std::vector<bool> valid;
    HadMath_douglasPeucker1(points, static_cast<uint32_t>(ptNum), threshold, valid);

    // 过滤z值
    if (zThreshold < basic::DOUBLE_MAX)
    {
        int lastValidIndex = 0;
        for (size_t i = 1; i < valid.size(); i++)
        {
            if (valid[i])
            {
                lastValidIndex = static_cast<int>(i);
            }
            else if (valid[i] == false &&
                     std::abs(points[i].z - points[lastValidIndex].z) >= zThreshold)
            {
                valid[i] = true;
                lastValidIndex = static_cast<int>(i);
            }
        }
    }

    // 滤出无效点,将剩下的点前置存储,并返回剩下的点的个数
    size_t num = 0;
    for (size_t i = 0; i < valid.size(); i++)
    {
        if (valid[i] == true)
        {
            points[num++] = points[i];
        }
    }
    return num;
}

} // namespace geo
} // namespace ns