paulmthompson / WhiskerToolbox / 16179668883

std::vector<double> compute_t_values(Line2D const & line) {

    if (line.empty()) {

        return {};

    std::vector<double> distances;

    distances.reserve(line.size());

    distances.push_back(0.0);  // First point has distance 0

    double total_distance = 0.0;

    for (size_t i = 1; i < line.size(); ++i) {

        double dx = line[i].x - line[i-1].x;

        double dy = line[i].y - line[i-1].y;

        double segment_distance = std::sqrt(dx*dx + dy*dy);

        total_distance += segment_distance;

        distances.push_back(total_distance);

    std::vector<double> t_values;

    t_values.reserve(line.size());

    if (total_distance > 0.0) {

        for (double d : distances) {

            t_values.push_back(d / total_distance);

        for (size_t i = 0; i < line.size(); ++i) {

            t_values.push_back(static_cast<double>(i) / static_cast<double>(line.size() > 1 ? line.size() - 1 : 1));

    return t_values;

}

std::vector<double> fit_single_dimension_polynomial_internal(

    if (dimension_coords.size() <= static_cast<size_t>(order) || t_values.size() != dimension_coords.size()) {

        return {}; // Not enough data or mismatched sizes

    arma::mat X_vandermonde(t_values.size(), order + 1);

    arma::vec Y_coords(const_cast<double*>(dimension_coords.data()), dimension_coords.size(), false); // Use const_cast and non-copy for efficiency

    for (size_t i = 0; i < t_values.size(); ++i) {

        for (int j = 0; j <= order; ++j) {

            X_vandermonde(i, j) = std::pow(t_values[i], j);

    arma::vec coeffs_arma;

    bool success = arma::solve(coeffs_arma, X_vandermonde, Y_coords);

    if (!success) {

        return {};

    return arma::conv_to<std::vector<double>>::from(coeffs_arma);

}