点列で表された道のりを、ある長さごとに区切るアルゴリズム C++

点列を表すのにOpenCVの cv::Point2f を使ったが、OpenCVライブラリは使ってないので適宜置き換えて欲しい。 最後から1つめと、最後の点との距離はlengthではない。

std::vector<cv::Point2f> cell_length(const std::vector<cv::Point2f>& points, double length) {
    assert(points.size() >= 2);

    std::vector<cv::Point2f> cell_points;
    cell_points.emplace_back(points.front());

    std::vector<double> vlength_rest;
    std::vector<double> vlength_total;

    for (int i = 0; i < points.size() - 1; i++) {
        cv::Point2f vec = points[i + 1] - points[i];
        vlength_rest.emplace_back(std::sqrt(vec.ddot(vec)));
        vlength_total.emplace_back(std::sqrt(vec.ddot(vec)));
    }
    double offset = 0;
    int ip = 0;

    while (true) {
        if (std::accumulate(vlength_rest.begin(), vlength_rest.end(), 0.0) == 0)break;
        if (offset + vlength_rest[ip] >= length) {
            vlength_rest[ip] -= (length - offset);

            cell_points.emplace_back(
                points[ip + 1] 
                * ((vlength_total[ip] - vlength_rest[ip]) / vlength_total[ip]) +
                points[ip] * (vlength_rest[ip] / vlength_total[ip]));

            if (vlength_rest[ip] <= 0) {
                vlength_rest[ip] = 0;
                ip++;
            }
            offset = 0;
        }
        else {
            offset += vlength_rest[ip];
            vlength_rest[ip] = 0;
            ip++;
        }
    }
    cell_points.emplace_back(points.back());
    return cell_points;
}