【基础函数-固定间隔采样轨迹点】

针对现有的曲线插值的方法（尤其是圆弧直线）规划出的泊车轨迹可能会因为路径的长短、生成的轨迹点的数量不同而导致轨迹点之间的间隔不同，不利于轨迹跟踪，因此开发按照固定间隔采样轨迹点的基础函数，直接上代码。

 /*按照固定间隔采样轨迹点*/
    std::vector<PathPoint> interpolateCubic(const std::vector<PathPoint> &originalTrajectory, float intervalSpacing)
    {
        std::vector<PathPoint> interpolatedTrajectory;
        int originalNumPoints = originalTrajectory.size();
        float totalLength = 0.0f;

        // Calculate the total length of the original trajectory
        for (int i = 0; i < originalNumPoints - 1; ++i)
        {
            float dx = originalTrajectory[i + 1].x - originalTrajectory[i].x;
            float dy = originalTrajectory[i + 1].y - originalTrajectory[i].y;
            totalLength += std::sqrt(dx * dx + dy * dy);
        }

        // Determine the number of intervals needed
        int numIntervals = std::ceil(totalLength / intervalSpacing);

        // Calculate the step size
        float stepSize = totalLength / numIntervals;

        // Calculate the cumulative distance at each point
        std::vector<float> cumulativeDistances(originalNumPoints, 0.0f);
        for (int i = 1; i < originalNumPoints; ++i)
        {
            float dx = originalTrajectory[i].x - originalTrajectory[i - 1].x;
            float dy = originalTrajectory[i].y - originalTrajectory[i - 1].y;
            cumulativeDistances[i] = cumulativeDistances[i - 1] + std::sqrt(dx * dx + dy * dy);
        }

        // Interpolate points based on the desired cumulative distance
        float currentDistance = 0.0f;
        int currentIndex = 0;
        for (int i = 0; i < numIntervals; ++i)
        {
            float targetDistance = currentDistance + stepSize;

            while (cumulativeDistances[currentIndex + 1] < targetDistance)
            {
                currentIndex++;
            }

            float weight = (targetDistance - cumulativeDistances[currentIndex]) / (cumulativeDistances[currentIndex + 1] - cumulativeDistances[currentIndex]);

            PathPoint interpolatedPoint;
            interpolatedPoint.x = originalTrajectory[currentIndex].x + weight * (originalTrajectory[currentIndex + 1].x - originalTrajectory[currentIndex].x);
            interpolatedPoint.y = originalTrajectory[currentIndex].y + weight * (originalTrajectory[currentIndex + 1].y - originalTrajectory[currentIndex].y);

            interpolatedTrajectory.push_back(interpolatedPoint);
            currentDistance = targetDistance;
        }

        return interpolatedTrajectory;
    }
    /*用线性插值相同间隔的轨迹点*/
    std::vector<PathPoint> interpolateTrajectory(const std::vector<PathPoint> &originalTrajectory, float spacing)
    {
        std::vector<PathPoint> interpolatedTrajectory;
        int numPoints = originalTrajectory.size();

        // Calculate the total length of the original trajectory
        float totalLength = 0.0f;
        for (int i = 0; i < numPoints - 1; ++i)
        {
            float dx = originalTrajectory[i + 1].x - originalTrajectory[i].x;
            float dy = originalTrajectory[i + 1].y - originalTrajectory[i].y;
            totalLength += std::sqrt(dx * dx + dy * dy);
        }

        // Calculate the number of interpolated points based on the desired spacing
        int numInterpolatedPoints = static_cast<int>(totalLength / spacing);

        // Interpolate the trajectory
        for (int i = 0; i < numPoints - 1; ++i)
        {
            float dx = originalTrajectory[i + 1].x - originalTrajectory[i].x;
            float dy = originalTrajectory[i + 1].y - originalTrajectory[i].y;
            float segmentLength = std::sqrt(dx * dx + dy * dy);
            int numSegments = static_cast<int>(segmentLength / spacing);

            for (int j = 0; j < numSegments; ++j)
            {
                float t = static_cast<float>(j) / numSegments;
                float interpolatedX = originalTrajectory[i].x + t * dx;
                float interpolatedY = originalTrajectory[i].y + t * dy;
                interpolatedTrajectory.push_back({interpolatedX, interpolatedY});
            }
        }

        // Add the last point of the original trajectory
        interpolatedTrajectory.push_back(originalTrajectory[numPoints - 1]);

        return interpolatedTrajectory;
    }