RefLine.cpp

#include "RefLine.h"
#include "Math.hpp"
#include "Utils.hpp"

#include <algorithm>
#include <cmath>
#include <functional>
#include <iterator>
#include <limits>
#include <stdexcept>
#include <utility>
#include <vector>

namespace odr
{

RefLine::RefLine(double length) : length(length) {}

RefLine::RefLine(const RefLine& other) : length(other.length), elevation_profile(other.elevation_profile)
{
    for (const auto& s0_geometry : other.s0_to_geometry)
        this->s0_to_geometry.emplace(s0_geometry.first, s0_geometry.second->clone());
}

std::set<const RoadGeometry*> RefLine::get_geometries() const
{
    std::set<const RoadGeometry*> geometries;
    for (const auto& s0_geometry : this->s0_to_geometry)
        geometries.insert(s0_geometry.second.get());
    return geometries;
}

std::set<RoadGeometry*> RefLine::get_geometries()
{
    std::set<RoadGeometry*> geometries;
    for (auto& s0_geometry : this->s0_to_geometry)
        geometries.insert(s0_geometry.second.get());
    return geometries;
}

double RefLine::get_geometry_s0(const double s) const
{
    if (this->s0_to_geometry.empty())
        return NAN;
    auto target_geom_iter = this->s0_to_geometry.upper_bound(s);
    if (target_geom_iter != s0_to_geometry.begin())
        target_geom_iter--;
    return target_geom_iter->first;
}

const RoadGeometry* RefLine::get_geometry(const double s) const
{
    const double geom_s0 = this->get_geometry_s0(s);
    if (std::isnan(geom_s0))
        return nullptr;
    return this->s0_to_geometry.at(geom_s0).get();
}

RoadGeometry* RefLine::get_geometry(const double s)
{
    RoadGeometry* road_geometry = const_cast<RoadGeometry*>(static_cast<const RefLine&>(*this).get_geometry(s));
    return road_geometry;
}

Vec3D RefLine::get_xyz(const double s) const
{
    const RoadGeometry* geom = this->get_geometry(s);

    Vec2D pt_xy{0, 0};
    if (geom)
        pt_xy = geom->get_xy(s);

    return Vec3D{pt_xy[0], pt_xy[1], this->elevation_profile.evaluate(s)};
}

Vec3D RefLine::derivative(const double s) const
{
    const RoadGeometry* geom = this->get_geometry(s);

    Vec2D d_xy{0, 0};
    if (geom)
        d_xy = geom->derivative(s);

    return Vec3D{d_xy[0], d_xy[1], this->elevation_profile.derivative(s)};
}

double RefLine::match(const double x, const double y) const
{
    if (this->length <= 0.0)
        return 0.0;

    std::function<double(double)> f_dist = [&](const double s)
    {
        const Vec3D pt = this->get_xyz(s);
        return euclDistance(Vec2D{pt[0], pt[1]}, {x, y});
    };

    // Coarse search to find the global minimum region.
    const double step = 2.0;
    double       min_s = 0.0;
    double       min_dist = std::numeric_limits<double>::max();

    for (double s = 0.0; s <= this->length; s += step)
    {
        const double dist = f_dist(s);
        if (dist < min_dist)
        {
            min_dist = dist;
            min_s = s;
        }
    }

    // Explicitly check the end point of the reference line.
    const double dist_end = f_dist(this->length);
    if (dist_end < min_dist)
    {
        min_dist = dist_end;
        min_s = this->length;
    }

    // Fine local search within a small window around min_s (which is guaranteed to be unimodal).
    const double search_start = std::max(0.0, min_s - step);
    const double search_end = std::min(this->length, min_s + step);

    return golden_section_search<double>(f_dist, search_start, search_end, 1e-2);
}

Line3D RefLine::get_line(const double s_start, const double s_end, const double eps) const
{
    std::set<double> s_vals = this->approximate_linear(eps, s_start, s_end);

    Line3D out_line;
    for (const double& s : s_vals)
        out_line.push_back(this->get_xyz(s));
    return out_line;
}

std::set<double> RefLine::approximate_linear(const double eps, const double s_start, const double s_end) const
{
    if ((s_start == s_end) || this->s0_to_geometry.empty())
        return {};

    auto s_end_geom_iter = this->s0_to_geometry.lower_bound(s_end);
    auto s_start_geom_iter = this->s0_to_geometry.upper_bound(s_start);
    if (s_start_geom_iter != s0_to_geometry.begin())
        s_start_geom_iter--;

    std::vector<double> s_vals{s_start};
    for (auto s0_geom_iter = s_start_geom_iter; s0_geom_iter != s_end_geom_iter; s0_geom_iter++)
    {
        const std::set<double> s_vals_geom = s0_geom_iter->second->approximate_linear(eps);
        if (s_vals_geom.size() < 2)
            throw std::runtime_error("expected at least two sample points");
        for (const double& s : s_vals_geom)
        {
            if (s > s_start && s < s_end)
                s_vals.push_back(s);
        }
        if (std::next(s0_geom_iter) != s_end_geom_iter)
            s_vals.pop_back();
    }

    std::set<double> s_vals_elevation = this->elevation_profile.approximate_linear(eps, s_start, s_end);
    for (const double& s : s_vals_elevation)
    {
        if (s > s_start && s < s_end)
            s_vals.push_back(s);
    }

    s_vals.push_back(s_end);

    std::set<double> s_vals_set(s_vals.begin(), s_vals.end());
    return s_vals_set;
}

} // namespace odr