算法说明
-
初始化：
-
从任意点开始(代码中选择第一个点)
-
找到它的最近邻作为第二个点
-
-
双向扩展：
-
向后扩展：从队尾的点出发，沿着前一点到当前点的方向，在允许角度偏差范围内寻找最近点
-
向前扩展：从队首的点出发，沿着第二个点到第一个点的方向，在允许角度偏差范围内寻找最近点
-
-
终止条件：
-
当两个方向都无法找到符合条件的点时，算法终止
-
-
关键参数：
-
angleThreshold_：控制搜索方向的角度偏差阈值(弧度) -
可以通过构造函数调整这个参数来适应不同曲线
-
部分关键代码如下：
#pragma once
#include<glm/glm.hpp>
#include<glm/ext.hpp>
#include<iostream>
#include<vector>
#include<string>
#include<cmath>
#include<algorithm>
#include<Eigen/Dense>
#include<map>
#include<random>
#include<numeric>
#include<deque>typedef glm::dvec3 Point3d;
typedef glm::dvec2 Point2d;
typedef glm::dvec2 Vec2d;
typedef glm::dvec3 Vec3d;class CurvePointSorter {
public:// 构造函数，可设置角度偏差阈值(弧度)explicit CurvePointSorter(double angleThreshold = M_PI / 3): angleThreshold_(angleThreshold) {}// 主接口：排序曲线点集std::vector<Point2d> sortPoints(const std::vector<Point2d>& points) {if (points.size() < 3) return points; // 点数太少直接返回std::vector<bool> used(points.size(), false);std::deque<Point2d> sortedPoints;// 1. 从任意点开始(这里选择第一个点)size_t firstIdx = 0;sortedPoints.push_back(points[firstIdx]);used[firstIdx] = true;// 2. 找到最近邻作为第二个点auto secondIdx = findClosestPoint(points, firstIdx, used);if (secondIdx == -1) return points; // 没有找到有效点sortedPoints.push_back(points[secondIdx]);used[secondIdx] = true;// 3. 双向扩展bool canExtend = true;while (canExtend) {canExtend = false;// 向后扩展(队尾方向)if (sortedPoints.size() > 1) {const Point2d& last = sortedPoints.back();const Point2d& prev = *(sortedPoints.rbegin() + 1);Vec2d dir = last - prev; // 前一点指向当前点的方向auto nextIdx = findDirectionalClosest(points, last, dir, used);if (nextIdx != -1) {sortedPoints.push_back(points[nextIdx]);used[nextIdx] = true;canExtend = true;}}// 向前扩展(队首方向)if (sortedPoints.size() > 1) {const Point2d& first = sortedPoints.front();const Point2d& second = *(sortedPoints.begin() + 1);Vec2d dir = first - second; // 第二点指向第一点的方向auto prevIdx = findDirectionalClosest(points, first, dir, used);if (prevIdx != -1) {sortedPoints.push_front(points[prevIdx]);used[prevIdx] = true;canExtend = true;}}}// 将deque转换为vector返回return std::vector<Point2d>(sortedPoints.begin(), sortedPoints.end());}private:double angleThreshold_; // 允许的角度偏差(弧度)// 找到距离给定点最近的点int findClosestPoint(const std::vector<Point2d>& points,size_t refIdx,const std::vector<bool>& used) {double minDist = std::numeric_limits<double>::max();int closestIdx = -1;for (size_t i = 0; i < points.size(); ++i) {if (!used[i] && i != refIdx) {double dist = glm::distance(points[refIdx], points[i]);if (dist < minDist) {minDist = dist;closestIdx = i;}}}return closestIdx;}// 在给定方向附近找到最近的点int findDirectionalClosest(const std::vector<Point2d>& points,const Point2d& refPoint,const Vec2d& direction,const std::vector<bool>& used) {double minDist = std::numeric_limits<double>::max();int closestIdx = -1;// 归一化方向向量Vec2d normDir = glm::normalize(direction); // { direction.x / dirLen, direction.y / dirLen };for (size_t i = 0; i < points.size(); ++i) {if (!used[i]) {Vec2d currDir = points[i] - refPoint;double currDirLen = std::sqrt(currDir.x * currDir.x + currDir.y * currDir.y);if (currDirLen > 0) {Vec2d normCurrDir = { currDir.x / currDirLen, currDir.y / currDirLen };// 计算两个方向向量的夹角(通过点积)double cosAngle = normDir.x * normCurrDir.x + normDir.y * normCurrDir.y;double angle = std::acos(std::min(1.0, std::max(-1.0, cosAngle)));// 检查角度是否在阈值范围内if (angle <= angleThreshold_) {double dist = glm::distance(refPoint,points[i]);if (dist < minDist) {minDist = dist;closestIdx = i;}}}}}return closestIdx;}
};
