diff --git a/editor/editorAlignment.cpp b/editor/editorAlignment.cpp index 232bc451..717bc43f 100644 --- a/editor/editorAlignment.cpp +++ b/editor/editorAlignment.cpp @@ -3974,6 +3974,218 @@ std::optional fit_arcs(design const &Single, std::vector fit_chain(design const &Base, std::vector const &Points, double const Speed) +{ + auto const n{Points.size()}; + if (n < 6) + return std::nullopt; + auto const start{plan_of(Base.start)}; + auto const startdirection{glm::normalize(Base.start_direction)}; + auto const end{plan_of(Base.end)}; + auto const enddirection{glm::normalize(Base.end_direction)}; + auto const travel{end - start}; + + // the curvature the points show, of the circle through each point and its neighbours, a little smoothed + std::vector bends(n, 0.0); + for (std::size_t i = 1; i + 1 < n; ++i) + { + auto const a{Points[i] - Points[i - 1]}; + auto const b{Points[i + 1] - Points[i]}; + auto const c{Points[i + 1] - Points[i - 1]}; + auto const lengths{glm::length(a) * glm::length(b) * glm::length(c)}; + if (lengths > 1e-9) + bends[i] = 2.0 * cross(a, b) / lengths; + } + std::vector curvature(n, 0.0); + double peak{0.0}; + for (std::size_t i = 1; i + 1 < n; ++i) + { + curvature[i] = (bends[i - 1] + 2.0 * bends[i] + bends[i + 1]) / ((i > 1 ? 1.0 : 0.0) + 2.0 + (i + 2 < n ? 1.0 : 0.0)); + peak = std::max(peak, std::abs(curvature[i])); + } + if (peak < 1.0 / kTraceRadiusMax) + return std::nullopt; + // the parts turning one way, of two points at least, the ones turning hardly at all between them + struct part + { + int side; + std::size_t first; + std::size_t last; + }; + std::vector parts; + for (std::size_t i = 1; i + 1 < n; ++i) + { + int const side{curvature[i] > 0.2 * peak ? 1 : curvature[i] < -0.2 * peak ? -1 : 0}; + if (side == 0) + continue; + if (false == parts.empty() && parts.back().side == side && parts.back().last + 1 == i) + parts.back().last = i; + else + parts.push_back({side, i, i}); + } + parts.erase(std::remove_if(parts.begin(), parts.end(), [](part const &Part) { return Part.last == Part.first; }), parts.end()); + // the same way again right after, with nothing straight between, is the same curve + for (std::size_t i = 1; i < parts.size();) + if (parts[i].side == parts[i - 1].side && parts[i].first <= parts[i - 1].last + 1) + { + parts[i - 1].last = parts[i].last; + parts.erase(parts.begin() + static_cast(i)); + } + else + ++i; + auto const count{parts.size()}; + if (count < 2 || count > 4 || std::none_of(parts.begin() + 1, parts.end(), [&](part const &Part) { return Part.side != (&Part - 1)->side; })) + return std::nullopt; + + // the parameters: of each line between two curves its direction and its distance from the start across it, then the logarithms of the + // radii, then the square roots of the lengths of the transitions + auto const lines{count - 1}; + auto const shaped = [&](std::vector const &Parameters, design &Trial) { + std::vector origins; + std::vector directions; + for (std::size_t j = 0; j < lines; ++j) + { + glm::dvec2 const direction{std::cos(Parameters[2 * j]), std::sin(Parameters[2 * j])}; + glm::dvec2 const across{-direction.y, direction.x}; + directions.push_back(direction); + origins.push_back(start + across * Parameters[2 * j + 1]); + } + // where two lines cross: along the first one, along the second + auto const crossing = [](glm::dvec2 const &A, glm::dvec2 const &Along, glm::dvec2 const &B, glm::dvec2 const &Other, double &First, double &Second) { + auto const determinant{cross(Along, Other)}; + if (std::abs(determinant) < 1e-9) + return false; + First = cross(B - A, Other) / determinant; + Second = cross(B - A, Along) / determinant; + return true; + }; + std::vector vertices; + double first; + double along; + if (false == crossing(start, startdirection, origins.front(), directions.front(), first, along) || first <= 0.0) + return false; + vertices.push_back(start + startdirection * first); + for (std::size_t j = 0; j + 1 < lines; ++j) + { + double a; + double b; + if (false == crossing(origins[j], directions[j], origins[j + 1], directions[j + 1], a, b)) + return false; + vertices.push_back(origins[j] + directions[j] * a); + } + double last; + if (false == crossing(end, -enddirection, origins.back(), directions.back(), last, along) || last <= 0.0) + return false; + vertices.push_back(end - enddirection * last); + // each vertex on from the one before along the line between them + for (std::size_t j = 0; j < lines; ++j) + if (glm::dot(vertices[j + 1] - vertices[j], directions[j]) <= 0.0) + return false; + Trial = Base; + Trial.vertices.assign(count, vertex{}); + Trial.vertices.front().offset = first; + Trial.vertices.back().offset = last; + for (std::size_t i = 1; i + 1 < count; ++i) + Trial.vertices[i].position = vertices[i]; + for (std::size_t i = 0; i < count; ++i) + { + auto const radius{std::exp(std::clamp(Parameters[2 * lines + i], std::log(kTraceRadiusMin), std::log(kTraceRadiusMax)))}; + auto const length{Parameters.size() > 2 * lines + count ? Parameters[2 * lines + count + i] * Parameters[2 * lines + count + i] : 0.0}; + plain_curve(Trial.vertices[i], radius, std::min({length, radius * vertex_deflection(Trial, i), longest_transition(Speed, radius, Trial.norms)}), Speed, Trial.norms); + } + return true; + }; + residuals_of const residuals{[&](std::vector const &Parameters, std::vector &Residuals) { + design trial; + return shaped(Parameters, trial) && stretch_residuals(trial, Points, Residuals); + }}; + // started from the line through the points where the curvature changes, the ones hardly turning there or the two either side + std::vector parameters; + for (std::size_t j = 0; j < lines; ++j) + { + auto const from{parts[j].last}; + auto const to{parts[j + 1].first}; + auto direction{Points[to] - Points[from]}; + if (glm::length(direction) < 1e-3) + return std::nullopt; + if (glm::dot(direction, travel) < 0.0) + direction = -direction; + auto const angle{std::atan2(direction.y, direction.x)}; + glm::dvec2 const across{-std::sin(angle), std::cos(angle)}; + parameters.push_back(angle); + parameters.push_back(glm::dot(across, 0.5 * (Points[from] + Points[to]) - start)); + } + for (auto const &each : parts) + { + double sum{0.0}; + for (auto i{each.first}; i <= each.last; ++i) + sum += std::abs(curvature[i]); + parameters.push_back(std::log(std::clamp(static_cast(each.last - each.first + 1) / std::max(sum, 1e-9), kTraceRadiusMin, kTraceRadiusMax))); + } + auto best{parameters}; + auto lowest{least_squares(best, residuals)}; + if (lowest == std::numeric_limits::max()) + return std::nullopt; + auto longer{best}; + for (std::size_t i = 0; i < count; ++i) + longer.push_back(std::sqrt(20.0)); + if (least_squares(longer, residuals) < lowest) + best = longer; + design chain; + if (false == shaped(best, chain)) + return std::nullopt; + auto const laid{compute(chain)}; + if (false == laid.valid || laid.curves.size() != count) + return std::nullopt; + // the radii and the transitions the curves were laid with, to the metre; where two curves of a reverse curve meet, filling the room + // between their vertices, a metre more doesn't fit: those as they were laid, a hair under, the design laid again giving no warning + auto const settled = [&](bool const Exact) { + auto settle{chain}; + for (auto const &curve : laid.curves) + { + if (curve.vertex < 0 || curve.vertex >= static_cast(count)) + return std::optional{}; + auto &curved{settle.vertices[curve.vertex]}; + auto const radius{Exact ? curve.radius * (1.0 - 1e-6) : std::max(kTraceRadiusMin, std::round(curve.radius))}; + auto const in{Exact ? curve.transition_in * (1.0 - 1e-6) : std::round(curve.transition_in)}; + auto const out{Exact ? curve.transition_out * (1.0 - 1e-6) : std::round(curve.transition_out)}; + plain_curve(curved, radius, std::min(in, out), Speed, settle.norms); + curved.transition_in = in; + curved.transition_out = out; + } + return std::optional{settle}; + }; + auto rounded{settled(false)}; + if (false == rounded.has_value()) + return std::nullopt; + chain = *rounded; + auto again{compute(chain)}; + if (again.valid && false == again.radius_warnings.empty()) + { + chain = *settled(true); + again = compute(chain); + } + if (false == again.valid || again.curves.size() != count || false == again.radius_warnings.empty()) + return std::nullopt; + for (auto const &curve : again.curves) + if (curve.radius < 1.05 * kTraceRadiusMin || curve.radius > 0.95 * kTraceRadiusMax || curve.deflection < 0.25 * kPi / 180.0 || curve.curvature_in > 0.0 || curve.curvature_out > 0.0) + return std::nullopt; + auto const curves{laid_curves(again)}; + for (std::size_t i = 1; i < curves.size(); ++i) + if (curves[i].side == curves[i - 1].side && curves[i].from - curves[i - 1].to < shortest_between(Base)) + return std::nullopt; + return stretch_of(chain, Points); +} + +} // namespace + trace_stretch fit_trace_stretch(design const &Base, std::vector const &Points, int const Radii, bool const Straights) { trace_stretch chosen; @@ -4171,6 +4383,10 @@ trace_stretch fit_trace_stretch(design const &Base, std::vector cons take(candidate); } } + + // a reverse curve, or more curves turning to and fro + if (false == chosen.valid || chosen.rms > kStretchClose) + take(fit_chain(Base, Points, speed)); return chosen; } diff --git a/tests/unit/test_alignment.cpp b/tests/unit/test_alignment.cpp index e9d37c7e..79acffe7 100644 --- a/tests/unit/test_alignment.cpp +++ b/tests/unit/test_alignment.cpp @@ -798,3 +798,87 @@ TEST_CASE("alignment: the pieces of a compound curve are never of no length, wha MESSAGE(valid << " valid designs"); CHECK(bad == 0); } + +namespace { +glm::dvec2 heading_of(double const Angle) { return heading(Angle); } + +// points every Spacing metres along a line from the origin along x of segments of the curvature going evenly from the first to the +// second (to the left positive) over the length, and two on the straight after it 30 and 60 m on; the end and the heading there +std::vector walked_points(std::vector> const &Segments, double const Spacing, line_end &End) { + std::vector points; + glm::dvec2 position{0.0}; + double heading{0.0}; + double chainage{0.0}; + auto next{Spacing}; + for (auto const &[from, to, length] : Segments) { + int const steps{std::max(1, static_cast(std::ceil(length / 0.05)))}; + auto const step{length / steps}; + auto const start{heading}; + auto const angle = [&, from = from, to = to, length = length](double const S) { return start + from * S + (to - from) * S * S / (2.0 * length); }; + for (int i = 0; i < steps; ++i) { + position += heading_of(angle((i + 0.5) * step)) * step; + chainage += step; + if (chainage >= next - 1e-6) { + points.push_back(position); + next += Spacing; + } + } + heading = angle(length); + } + points.push_back(position + heading_of(heading) * 30.0); + points.push_back(position + heading_of(heading) * 60.0); + End.position = position + heading_of(heading) * 60.0; + End.heading = heading; + return points; +} +} // namespace + +TEST_CASE("alignment: reverse curves traced in the mode of a curve, with no straight between them or with one, two or three curves") { + double const k{1.0 / 300.0}; + struct shape { + char const *name; + std::vector> segments; + double speed; + std::vector radii; + double noise; + }; + std::vector const shapes{ + {"transitions meeting at the inflection", {{0, 0, 20}, {0, k, 30}, {k, k, 50}, {k, -k, 60}, {-k, -k, 50}, {-k, 0, 30}}, 100.0, {300.0, 300.0}, 0.0}, + {"the same clicked 5 cm off", {{0, 0, 20}, {0, k, 30}, {k, k, 50}, {k, -k, 60}, {-k, -k, 50}, {-k, 0, 30}}, 60.0, {300.0, 300.0}, 0.05}, + {"a straight between", {{0, 0, 20}, {0, k, 30}, {k, k, 50}, {k, 0, 30}, {0, 0, 30}, {0, -k, 30}, {-k, -k, 50}, {-k, 0, 30}}, 60.0, {300.0, 300.0}, 0.0}, + {"arcs meeting", {{0, 0, 20}, {k, k, 60}, {-k, -k, 60}}, 40.0, {300.0, 300.0}, 0.0}, + {"other radii", {{0, 0, 20}, {0, 0.5 * k, 40}, {0.5 * k, 0.5 * k, 60}, {0.5 * k, -0.75 * k, 70}, {-0.75 * k, -0.75 * k, 40}, {-0.75 * k, 0, 40}}, 80.0, {600.0, 400.0}, 0.0}, + {"the ends not parallel", {{0, 0, 20}, {0, k, 30}, {k, k, 90}, {k, -k, 60}, {-k, -k, 30}, {-k, 0, 30}}, 60.0, {300.0, 300.0}, 0.0}, + {"left, right, left", {{0, 0, 20}, {0, k, 30}, {k, k, 40}, {k, -k, 60}, {-k, -k, 40}, {-k, k, 60}, {k, k, 40}, {k, 0, 30}}, 60.0, {300.0, 300.0, 300.0}, 0.0}, + {"left, right, left clicked 5 cm off", {{0, 0, 20}, {0, k, 30}, {k, k, 40}, {k, -k, 60}, {-k, -k, 40}, {-k, k, 60}, {k, k, 40}, {k, 0, 30}}, 60.0, {300.0, 300.0, 300.0}, 0.05}, + }; + for (auto const &each : shapes) { + std::string const name{each.name}; + CAPTURE(name); + line_end end; + auto points{walked_points(each.segments, 10.0, end)}; + // off to and fro across the way, as clicks are + for (std::size_t i = 0; i < points.size(); ++i) + points[i].y += each.noise * (i % 3 == 0 ? 1.0 : i % 3 == 1 ? -0.6 : 0.2); + alignment::design base; + base.speed = each.speed; + base.start = {0.0, 0.0, 0.0}; + base.start_direction = heading(0.0); + base.end = {end.position.x, 0.0, end.position.y}; + base.end_direction = heading(end.heading); + auto const fit{alignment::fit_trace_stretch(base, points, 0, true)}; + REQUIRE(fit.valid); + REQUIRE(fit.vertices.size() == each.radii.size()); + for (std::size_t i = 0; i < each.radii.size(); ++i) + CHECK(fit.vertices[i].radius == doctest::Approx(each.radii[i]).epsilon(0.03)); + CHECK(fit.rms < 0.02 + each.noise); + // laid as the design of the curves fitted, with no warning of radii made smaller + auto laid{base}; + laid.vertices = fit.vertices; + for (std::size_t i = 1; i + 1 < laid.vertices.size(); ++i) + laid.vertices[i].position = fit.vertices[i].position; + auto const line{alignment::compute(laid)}; + CHECK(line.valid); + CHECK(line.radius_warnings.empty()); + } +}