17
0
mirror of https://github.com/MaSzyna-EU07/maszyna.git synced 2026-10-26 07:59:30 +01:00
Files
maszyna/editor/editorAlignment.cpp

3761 lines
152 KiB
C++

/*
This Source Code Form is subject to the
terms of the Mozilla Public License, v.
2.0. If a copy of the MPL was not
distributed with this file, You can
obtain one at
http:
*/
#include "stdafx.h"
#include "editor/editorAlignment.hpp"
#include "editor/editorFormat.hpp"
#include "editor/editorGeometry.hpp"
#include "utilities/translation.h"
#include <algorithm>
#include <array>
#include <cmath>
#include <cstdio>
#include <functional>
#include <limits>
#include <optional>
namespace alignment
{
namespace
{
double const kGravity{9.81};
double const kPi{3.14159265358979323846};
using geometry::cross;
using geometry::plan_of;
using geometry::turned;
glm::dvec2 perpendicular(glm::dvec2 const &Vector)
{
return {-Vector.y, Vector.x};
}
double ceil_to(double const Value, double const Step)
{
return std::ceil(Value / Step - 1e-9) * Step;
}
struct local_point
{
double x{0.0};
double y{0.0};
double angle{0.0};
};
local_point transition_point(transition_shape const Shape, double const Length, double const Radius, double const Parameter)
{
if (Length <= 0.0 || Radius <= 0.0)
return {};
if (Shape == transition_shape::cubic_parabola)
{
auto const x{Length * Parameter};
return {x, x * x * x / (6.0 * Radius * Length), std::atan(x * x / (2.0 * Radius * Length))};
}
auto const distance{Length * Parameter};
auto const angle = [&](double const S) { return S * S / (2.0 * Radius * Length); };
// Simpson over 64 steps at most keeps a transition of a radian within a millimetre, however long
int const steps{std::clamp(static_cast<int>(std::ceil(distance / 0.5)) * 2, 8, 64)};
double const h{distance / steps};
double x{0.0};
double y{0.0};
for (int i = 0; i <= steps; ++i)
{
double const weight = (i == 0 || i == steps) ? 1.0 : (i % 2 ? 4.0 : 2.0);
auto const a{angle(i * h)};
x += weight * std::cos(a);
y += weight * std::sin(a);
}
return {x * h / 3.0, y * h / 3.0, angle(distance)};
}
local_point spiral_point(double const Start, double const End, double const Length, double const Distance)
{
if (Distance <= 0.0)
return {};
// an arc, or a straight, has its point at once
if (Start == End)
{
auto const angle{Start * Distance};
if (std::abs(angle) < 1e-9)
return {Distance, 0.0, 0.0};
return {std::sin(angle) / Start, (1.0 - std::cos(angle)) / Start, angle};
}
auto const heading = [&](double const S) { return Start * S + (End - Start) * S * S / (2.0 * std::max(Length, 1e-9)); };
int const steps{std::clamp(static_cast<int>(std::ceil(Distance / 0.5)) * 2, 8, 64)};
double const h{Distance / steps};
double x{0.0};
double y{0.0};
for (int i = 0; i <= steps; ++i)
{
double const weight = (i == 0 || i == steps) ? 1.0 : (i % 2 ? 4.0 : 2.0);
auto const a{heading(i * h)};
x += weight * std::cos(a);
y += weight * std::sin(a);
}
return {x * h / 3.0, y * h / 3.0, heading(Distance)};
}
struct compound_curve
{
// the curvature at the start and at the end, the length, the cant at the start and at the end
std::vector<std::array<double, 5>> segments;
local_point end;
double shortened{1.0};
};
// Cants: of the arcs, the cants of the segments stay 0 without them
compound_curve build_compound(double const Deflection, std::vector<compound_arc> const &Arcs, double const Lengthin, double const Lengthout, std::vector<double> const &Cants = {})
{
compound_curve result;
if (Arcs.empty())
return result;
std::vector<double> curvatures;
for (auto const &arc : Arcs)
curvatures.push_back(1.0 / std::max(1.0, arc.radius));
auto const cant = [&](std::size_t const Arc) { return Arc < Cants.size() ? Cants[Arc] : 0.0; };
auto transitions{curvatures.front() * Lengthin + curvatures.back() * Lengthout};
for (std::size_t i = 1; i < Arcs.size(); ++i)
transitions += (curvatures[i - 1] + curvatures[i]) * std::max(0.0, Arcs[i].transition);
transitions *= 0.5;
if (transitions > Deflection)
result.shortened = Deflection / transitions * (1.0 - 1e-9);
auto const f{result.shortened};
auto const arcs{std::max(0.0, Deflection - transitions * f)};
double total{0.0};
for (auto const &arc : Arcs)
total += std::max(0.0, arc.share);
std::vector<std::array<double, 5>> segments{{0.0, curvatures.front(), Lengthin * f, 0.0, cant(0)}};
for (std::size_t i = 0; i < Arcs.size(); ++i)
{
if (i > 0)
segments.push_back({curvatures[i - 1], curvatures[i], std::max(0.0, Arcs[i].transition) * f, cant(i - 1), cant(i)});
auto const share{total > 0.0 ? std::max(0.0, Arcs[i].share) / total : 1.0 / static_cast<double>(Arcs.size())};
segments.push_back({curvatures[i], curvatures[i], share * arcs / curvatures[i], cant(i), cant(i)});
}
segments.push_back({curvatures.back(), 0.0, Lengthout * f, cant(Arcs.size() - 1), 0.0});
double x{0.0};
double y{0.0};
double heading{0.0};
for (auto const &segment : segments)
{
if (segment[2] < 1e-6)
continue;
result.segments.push_back(segment);
auto const local{spiral_point(segment[0], segment[1], segment[2], segment[2])};
x += local.x * std::cos(heading) - local.y * std::sin(heading);
y += local.x * std::sin(heading) + local.y * std::cos(heading);
heading += local.angle;
}
result.end = {x, y, heading};
return result;
}
// the cants of the arcs, each its own or Cant at the sharpest of them in proportion to the curvature
std::vector<double> arc_cants(std::vector<compound_arc> const &Arcs, double const Cant)
{
auto sharpest{std::numeric_limits<double>::max()};
for (auto const &arc : Arcs)
sharpest = std::min(sharpest, std::max(1.0, arc.radius));
std::vector<double> cants;
for (auto const &arc : Arcs)
cants.push_back(arc.cant >= 0.0 ? arc.cant : std::max(0.0, Cant) * sharpest / std::max(1.0, arc.radius));
return cants;
}
void element_point(element const &Element, transition_shape const Shape, double const Distance, glm::dvec2 &Position, glm::dvec2 &Direction)
{
switch (Element.kind)
{
case element_kind::straight:
Position = Element.origin + Element.direction * Distance;
Direction = Element.direction;
break;
case element_kind::arc:
{
auto const angle{Element.turn * Distance / Element.radius};
Position = Element.origin + turned(Element.normal, angle);
Direction = turned(Element.direction, angle);
break;
}
case element_kind::transition_in:
{
auto const local{transition_point(Shape, Element.transition, Element.radius, Distance / Element.transition)};
Position = Element.origin + Element.direction * local.x + Element.normal * local.y;
Direction = Element.direction * std::cos(local.angle) + Element.normal * std::sin(local.angle);
break;
}
case element_kind::spiral:
{
auto const local{spiral_point(Element.curvature_start, Element.curvature_end, Element.length, Distance)};
Position = Element.origin + Element.direction * local.x + Element.normal * local.y;
Direction = Element.direction * std::cos(local.angle) + Element.normal * std::sin(local.angle);
break;
}
case element_kind::transition_out:
{
auto const local{transition_point(Shape, Element.transition, Element.radius, 1.0 - Distance / Element.transition)};
Position = Element.origin + Element.direction * local.x + Element.normal * local.y;
Direction = -(Element.direction * std::cos(local.angle) + Element.normal * std::sin(local.angle));
break;
}
}
}
double element_cant(element const &Element, double const Distance)
{
switch (Element.kind)
{
case element_kind::transition_in: return Element.cant * Distance / Element.transition;
case element_kind::transition_out: return Element.cant * (1.0 - Distance / Element.transition);
case element_kind::arc: return Element.cant;
case element_kind::spiral: return Element.cant_start + (Element.cant_end - Element.cant_start) * std::clamp(Distance / std::max(Element.length, 1e-9), 0.0, 1.0);
default: return 0.0;
}
}
element const &element_at(result const &Result, double const Chainage)
{
auto const lookup = std::upper_bound(Result.elements.begin(), Result.elements.end(), Chainage, [](double const Value, element const &Element) { return Value < Element.chainage; });
return lookup == Result.elements.begin() ? Result.elements.front() : *std::prev(lookup);
}
// the cant of the line at the chainage, signed with the turn of its curve; where it changes all at once there, the one before the
// chainage or the one after it
double signed_cant(result const &Result, element const &Element, double const Distance, bool const Before)
{
auto const &cants{Result.cants};
if (cants.empty())
return Element.turn * element_cant(Element, Distance);
auto const chainage{Element.chainage + Distance};
auto const next{Before ? std::lower_bound(cants.begin(), cants.end(), chainage, [](glm::dvec2 const &Point, double const Value) { return Point.x < Value; })
: std::upper_bound(cants.begin(), cants.end(), chainage, [](double const Value, glm::dvec2 const &Point) { return Value < Point.x; })};
if (next == cants.begin())
return cants.front().y;
if (next == cants.end())
return cants.back().y;
auto const previous{std::prev(next)};
auto const span{next->x - previous->x};
return span < 1e-9 ? next->y : previous->y + (next->y - previous->y) * (chainage - previous->x) / span;
}
glm::dvec2 blossom(std::array<glm::dvec2, 4> const &Points, double const A, double const B, double const C)
{
glm::dvec2 r[3], s[2];
for (int i = 0; i < 3; ++i)
r[i] = glm::mix(Points[i], Points[i + 1], A);
for (int i = 0; i < 2; ++i)
s[i] = glm::mix(r[i], r[i + 1], B);
return glm::mix(s[0], s[1], C);
}
std::size_t element_pieces(element const &Element, transition_shape const Shape, int const Transitions, double const Arcangle)
{
if (Element.length < 1e-3)
return 0;
switch (Element.kind)
{
case element_kind::arc:
return static_cast<std::size_t>(std::max(1.0, std::ceil(Element.length / Element.radius / (Arcangle * kPi / 180.0) - 1e-9)));
case element_kind::transition_in:
case element_kind::transition_out:
return std::max<std::size_t>(std::max(1, Transitions), Shape == transition_shape::cubic_parabola ? 1 : static_cast<std::size_t>(std::max(1.0, std::ceil(Element.length / 20.0 - 1e-9))));
case element_kind::spiral:
{
auto const turn{(Element.curvature_start + Element.curvature_end) * 0.5 * Element.length};
auto const arcs{std::max(1.0, std::ceil(turn / (Arcangle * kPi / 180.0) - 1e-9))};
if (Element.curvature_start == Element.curvature_end)
return static_cast<std::size_t>(arcs);
auto const count{static_cast<std::size_t>(std::max(arcs, std::ceil(Element.length / 20.0 - 1e-9)))};
return std::max<std::size_t>(count, std::max(1, Transitions));
}
default:
return 1;
}
}
}
double unbalanced_acceleration(double const Speed, double const Radius, double const Cant, limits const &Limits)
{
if (Radius <= 0.0)
return 0.0;
return Speed * Speed / (12.96 * Radius) - kGravity * Cant / Limits.gauge;
}
std::vector<double> compound_cants(vertex const &Vertex)
{
if (false == Vertex.compound || Vertex.arcs.empty())
return {std::max(0.0, Vertex.cant)};
std::vector<compound_arc> arcs{{Vertex.radius, 0.0, Vertex.share, Vertex.first_cant}};
arcs.insert(arcs.end(), Vertex.arcs.begin(), Vertex.arcs.end());
return arc_cants(arcs, Vertex.cant);
}
double transition_length(double const Speed, double const Radius, double const Cant, limits const &Limits)
{
double length{0.0};
if (Cant > 0.0)
{
length = std::max(length, Speed * Cant / (3.6 * Limits.cant_rate));
length = std::max(length, Cant / 1000.0 * Limits.ramp_factor * Speed);
}
auto const unbalanced{std::max(0.0, unbalanced_acceleration(Speed, Radius, Cant, Limits))};
length = std::max(length, unbalanced * Speed / (3.6 * Limits.jerk));
return length > 0.0 ? ceil_to(length, 5.0) : 0.0;
}
double transition_between(double const Speed, double const Curvature0, double const Cant0, double const Curvature1, double const Cant1, limits const &Limits)
{
auto const cant{std::abs(Cant1 - Cant0)};
double length{0.0};
if (cant > 0.0)
{
length = std::max(length, Speed * cant / (3.6 * Limits.cant_rate));
length = std::max(length, cant / 1000.0 * Limits.ramp_factor * Speed);
}
auto const unbalanced = [&](double const Curvature, double const Cant) { return Speed * Speed / 12.96 * Curvature - kGravity * Cant / Limits.gauge; };
length = std::max(length, std::abs(unbalanced(Curvature1, Cant1) - unbalanced(Curvature0, Cant0)) * Speed / (3.6 * Limits.jerk));
return length > 0.0 ? ceil_to(length, 5.0) : 0.0;
}
recommendation recommend(double const Speed, double const Radius, limits const &Limits)
{
recommendation result;
auto const factor{Limits.gauge / (kGravity * 12.96)};
auto const allowance{Limits.gauge * Limits.unbalanced / kGravity};
result.radius_min = factor * Speed * Speed / (Limits.cant_max + allowance);
if (Radius <= 0.0)
return result;
result.cant_equilibrium = factor * Speed * Speed / Radius;
result.cant_min = std::max(0.0, result.cant_equilibrium - allowance);
if (false == Limits.checked)
return result;
result.cant = std::min(Limits.cant_max, ceil_to(std::max(result.cant_min, result.cant_equilibrium * 2.0 / 3.0), 5.0));
result.transition = std::min(5000.0, transition_length(Speed, Radius, result.cant, Limits));
return result;
}
namespace
{
// the tangents of a curve of the radius with transitions of the lengths, from the vertex back along the straight in and on along the
// one out
void curve_tangents(transition_shape const Shape, double const Lengthin, double const Lengthout, double const Radius, glm::dvec2 const &Directionin, glm::dvec2 const &Directionout, int const Side,
double &Tangentin, double &Tangentout)
{
auto const endin{transition_point(Shape, Lengthin, Radius, 1.0)};
auto const endout{transition_point(Shape, Lengthout, Radius, 1.0)};
auto const a{Radius + endin.y - Radius * (1.0 - std::cos(endin.angle))};
auto const b{Radius + endout.y - Radius * (1.0 - std::cos(endout.angle))};
auto const normalin{perpendicular(Directionin) * static_cast<double>(Side)};
auto const normalout{perpendicular(Directionout) * static_cast<double>(Side)};
auto const determinant{cross(normalin, normalout)};
glm::dvec2 const offset{(a * normalout.y - normalin.y * b) / determinant, (normalin.x * b - a * normalout.x) / determinant};
auto const curvestart{offset - normalin * a - Directionin * (endin.x - Radius * std::sin(endin.angle))};
auto const curveend{offset - normalout * b + Directionout * (endout.x - Radius * std::sin(endout.angle))};
Tangentin = std::max(0.0, -glm::dot(curvestart, Directionin));
Tangentout = std::max(0.0, glm::dot(curveend, Directionout));
}
}
// each of the two transitions turns half of the deflection, ending at the curvature of the radius
double spirals_length(transition_shape const Shape, double const Deflection, double const Radius)
{
auto const half{std::clamp(Deflection * 0.5, 0.0, 0.5 * kPi - 1e-6)};
return Shape == transition_shape::clothoid ? 2.0 * Radius * half : 2.0 * Radius * std::tan(half);
}
bool meets_norms(curve_report const &Curve, design const &Design)
{
auto const &norms{Design.norms};
auto sharpest{Curve.radius};
for (auto const radius : Curve.radii)
sharpest = std::min(sharpest, radius);
if (Curve.unbalanced > norms.unbalanced + 1e-6 || Curve.cant > norms.cant_max + 1e-6)
return false;
if (false == Curve.spirals && sharpest < Curve.recommended.radius_min - 1e-6)
return false;
if (Curve.radii.size() > 1 && Curve.cants.size() == Curve.radii.size())
// a compound curve ramps the cant of its first arc in and the one of its last arc out
return Curve.transition_in + 1e-6 >= transition_length(Design.speed, Curve.radii.front(), Curve.cants.front(), norms) &&
Curve.transition_out + 1e-6 >= transition_length(Design.speed, Curve.radii.back(), Curve.cants.back(), norms);
auto const required{transition_length(Design.speed, sharpest, Curve.cant, norms)};
return Curve.transition_in + 1e-6 >= required && Curve.transition_out + 1e-6 >= required;
}
namespace
{
// the length of a straight cant ramp the norms ask for to raise the cant at the speed: slow enough in time and along the track
double ramp_length(double const Speed, double const Cant, limits const &Limits)
{
if (Cant <= 0.0)
return 0.0;
return ceil_to(std::max(Speed * Cant / (3.6 * Limits.cant_rate), Cant / 1000.0 * Limits.ramp_factor * Speed), 5.0);
}
// the cant along the line: in each curve as its elements carry it, raised from 0 and lowered back to it on its ramps, which go along
// its transition curves unless the vertex gives them a place and a length of their own
void make_cants(design const &Design, result &R)
{
R.cants.clear();
R.ramps.clear();
auto const push = [&](double const Chainage, double const Cant) {
if (false == R.cants.empty() && std::abs(R.cants.back().x - Chainage) < 1e-6 && std::abs(R.cants.back().y - Cant) < 1e-6)
return;
R.cants.push_back({Chainage, Cant});
};
double reached{0.0}; // where the cant of the curve before is down to 0 again
for (std::size_t k = 0; k < Design.vertices.size(); ++k)
{
auto const &vertex{Design.vertices[k]};
int first{-1};
int last{-1};
for (std::size_t i = 0; i < R.elements.size(); ++i)
if (R.elements[i].vertex == static_cast<int>(k) && R.elements[i].length > 1e-6)
{
first = first < 0 ? static_cast<int>(i) : first;
last = static_cast<int>(i);
}
if (first < 0)
continue;
auto const &head{R.elements[first]};
auto const &tail{R.elements[last]};
auto const start{head.chainage};
auto const end{tail.chainage + tail.length};
auto const turn{static_cast<double>(head.turn)};
// the transitions at the ends: the first and the last element, where the curvature grows from 0 and goes back to it
auto const entry{head.kind == element_kind::transition_in || (head.kind == element_kind::spiral && head.curvature_start == 0.0 && head.curvature_end > 0.0) ? head.length : 0.0};
auto const exit{tail.kind == element_kind::transition_out || (tail.kind == element_kind::spiral && tail.curvature_end == 0.0 && tail.curvature_start > 0.0) ? tail.length : 0.0};
auto const cantin{element_cant(head, entry)};
auto const cantout{element_cant(tail, tail.length - exit)};
if (std::max(cantin, cantout) < 0.5)
{
reached = end;
continue;
}
result::cant_ramp in{static_cast<int>(k), start, start + entry, cantin, vertex.ramp_in_length >= 0.0};
result::cant_ramp out{static_cast<int>(k), end - exit, end, cantout, vertex.ramp_out_length >= 0.0};
if (in.own)
{
in.from = start + vertex.ramp_in_offset;
in.to = in.from + vertex.ramp_in_length;
}
if (out.own)
{
out.to = end - vertex.ramp_out_offset;
out.from = out.to - vertex.ramp_out_length;
}
// the ramps kept off the curve before, within the line and off each other
if (in.from < reached - 1e-6)
{
R.warnings.emplace_back(R.ramps.empty() ? format(STR_C("Vertex %zu: the cant ramp in reaches back past the start of the line, cut %.1f m shorter"), k + 1, reached - in.from) :
format(STR_C("Vertex %zu: the cant ramp in reaches back into the curve before it, cut %.1f m shorter"), k + 1, reached - in.from));
in.from = std::min(reached, in.to);
}
if (out.to > R.length + 1e-6)
{
R.warnings.emplace_back(format(STR_C("Vertex %zu: the cant ramp out reaches past the end of the line, cut %.1f m shorter"), k + 1, out.to - R.length));
out.to = std::max(R.length, out.from);
}
if (in.to > out.from + 1e-6)
{
R.warnings.emplace_back(format(STR_C("Vertex %zu: the cant ramps in and out of the curve overlap by %.1f m"), k + 1, in.to - out.from));
auto const middle{(in.to + out.from) * 0.5};
in.to = std::max(in.from, middle);
out.from = std::min(out.to, middle);
}
auto const speed{std::max(1.0, Design.speed)};
for (auto const *ramp : {&in, &out})
if (ramp->own && Design.norms.checked)
if (auto const required{ramp_length(speed, ramp->cant, Design.norms)}; ramp->to - ramp->from + 1e-6 < required)
R.warnings.emplace_back(format(STR_C("Vertex %zu: the cant ramp of %.1f m is shorter than the %.0f m a cant of %.0f mm asks for"), k + 1, ramp->to - ramp->from, required, ramp->cant));
// the curve between its ramps as its elements carry the cant, the transitions at its ends with the full cant of the curve where
// the ramps went off them
push(in.from, 0.0);
push(in.to, turn * cantin);
auto const low{std::max(in.to, start + entry)};
auto const high{std::min(out.from, end - exit)};
if (high > low + 1e-6)
{
push(low, turn * cantin);
for (auto i{first}; i <= last; ++i)
{
auto const &e{R.elements[i]};
for (auto const distance : {0.0, e.length})
if (auto const chainage{e.chainage + distance}; chainage > low + 1e-6 && chainage < high - 1e-6)
push(chainage, turn * element_cant(e, distance));
}
push(high, turn * cantout);
}
push(out.from, turn * cantout);
push(out.to, 0.0);
R.ramps.push_back(in);
R.ramps.push_back(out);
reached = out.to;
// the time the cant takes to rise along the ramps of their own
for (auto &curve : R.curves)
if (curve.vertex == static_cast<int>(k))
{
if (in.own && in.to > in.from)
curve.cant_rate_in = cantin * speed / 3.6 / (in.to - in.from);
if (out.own && out.to > out.from)
curve.cant_rate_out = cantout * speed / 3.6 / (out.to - out.from);
}
}
std::stable_sort(R.cants.begin(), R.cants.end(), [](glm::dvec2 const &A, glm::dvec2 const &B) { return A.x < B.x; });
}
result fit_between(design const &Design)
{
result r;
r.shape = Design.shape;
r.gauge = Design.norms.gauge;
r.start = Design.start;
r.end = Design.end;
auto const startreserve{std::max(0.0, Design.start_reserve)};
auto const endreserve{std::max(0.0, Design.end_reserve)};
auto const start{plan_of(Design.start)};
auto const end{plan_of(Design.end)};
if (glm::length(Design.start_direction) < 1e-9 || glm::length(Design.end_direction) < 1e-9)
{
r.errors.emplace_back(STR_C("The direction of an end of the fragment is undefined, its end path has zero length"));
return r;
}
auto const startdirection{glm::normalize(Design.start_direction)};
auto const enddirection{glm::normalize(Design.end_direction)};
auto const count{Design.vertices.size()};
r.vertices.resize(count);
if (count == 0)
{
auto const offset{end - start};
if (std::abs(cross(startdirection, offset)) > 0.01 || glm::dot(startdirection, enddirection) < std::cos(1e-4) || glm::dot(startdirection, offset) <= 0.0)
r.errors.emplace_back(STR_C("The ends don't lie on a common straight line, add a vertex"));
}
else if (count == 1)
{
auto const offset{end - start};
auto const determinant{cross(startdirection, enddirection)};
if (std::abs(determinant) < 1e-9)
{
r.errors.emplace_back(STR_C("Tangents of the ends are parallel, use two vertices"));
}
else
{
auto const t{cross(offset, enddirection) / determinant};
auto const u{cross(startdirection, offset) / determinant};
if ((t <= 0.0 || u <= 0.0) && false == Design.vertices.front().reverse_turn)
r.errors.emplace_back(STR_C("Tangents of the ends intersect outside of the fragment, turn the longer way or use two vertices"));
r.vertices[0] = start + startdirection * t;
}
}
else
{
for (std::size_t i = 1; i + 1 < count; ++i)
r.vertices[i] = Design.vertices[i].position;
r.vertices.front() = start + startdirection * Design.vertices.front().offset;
r.vertices.back() = end - enddirection * Design.vertices.back().offset;
}
if (false == r.errors.empty())
return r;
std::vector<glm::dvec2> polygon{start};
polygon.insert(polygon.end(), r.vertices.begin(), r.vertices.end());
polygon.push_back(end);
for (std::size_t i = 0; i + 1 < polygon.size(); ++i)
{
if (glm::distance(polygon[i], polygon[i + 1]) < 1e-3)
{
r.errors.emplace_back(format(STR_C("Tangent %zu has zero length"), i + 1));
return r;
}
}
double chainage{0.0};
auto const push = [&](element Element) {
Element.chainage = chainage;
chainage += Element.length;
r.elements.push_back(Element);
};
auto const push_straight = [&](glm::dvec2 const &From, glm::dvec2 const &Direction, double const Length) {
if (Length <= 1e-6)
return;
element straight;
straight.kind = element_kind::straight;
straight.origin = From;
straight.direction = Direction;
straight.length = Length;
push(straight);
};
auto const starts_early = [&](double const Straight) {
if (Straight < 0.0)
r.start_extension = -Straight;
if (Straight >= -1e-4 || -Straight <= startreserve + 1e-4)
return;
if (startreserve > 0.0)
r.errors.emplace_back(format(STR_C("Vertex 1: the curve starts %.2f m before the adjoining straight"), -Straight - startreserve));
else
r.errors.emplace_back(format(STR_C("Vertex 1: the curve starts %.2f m before the fixed start"), -Straight));
};
struct fitted
{
double radius{0.0};
double transition_in{0.0};
double transition_out{0.0};
double tangent_in{0.0};
double tangent_out{0.0};
double deflection{0.0};
double cant{0.0};
bool compound{false};
bool spirals{false};
std::vector<compound_arc> arcs;
std::function<void(double, double &, double &)> tangents;
};
auto const scaled = [](std::vector<compound_arc> Arcs, double const Scale) {
for (auto &arc : Arcs)
{
arc.radius *= Scale;
arc.transition *= Scale;
}
return Arcs;
};
std::vector<fitted> fits(count);
for (std::size_t k = 0; k < count; ++k)
{
auto const &vertex{Design.vertices[k]};
auto &fit{fits[k]};
fit.radius = std::max(1.0, vertex.radius);
fit.transition_in = std::max(0.0, vertex.transition_in);
fit.transition_out = std::max(0.0, vertex.transition_out);
auto const directionin{glm::normalize(polygon[k + 1] - polygon[k])};
auto const directionout{glm::normalize(polygon[k + 2] - polygon[k + 1])};
auto const turn{cross(directionin, directionout)};
auto const geometric{std::atan2(std::abs(turn), glm::dot(directionin, directionout))};
auto const reverse{Design.vertices[k].reverse_turn};
auto const deflection{reverse ? 2.0 * kPi - geometric : geometric};
int const side{(turn > 0.0) != reverse ? 1 : -1};
if (deflection < 1e-7)
continue;
if (geometric > kPi - 1e-3 || (reverse && geometric < 1e-3))
{
r.errors.emplace_back(format(STR_C("Vertex %zu: a turn of about 180 or 360 degrees needs one more vertex"), k + 1));
return r;
}
if (vertex.compound && false == vertex.arcs.empty())
{
fit.compound = true;
fit.arcs = {{fit.radius, 0.0, vertex.share, vertex.first_cant}};
for (auto const &arc : vertex.arcs)
fit.arcs.push_back({std::max(1.0, arc.radius), std::max(0.0, arc.transition), arc.share, arc.cant});
fit.deflection = deflection;
fit.tangents = [&, k, directionin, directionout, side, scaled](double const Radius, double &Tangentin, double &Tangentout) {
auto const &own{fits[k]};
auto const scale{Radius / own.radius};
auto const shape{build_compound(own.deflection, scaled(own.arcs, scale), own.transition_in * scale, own.transition_out * scale)};
auto const normalin{perpendicular(directionin) * static_cast<double>(side)};
auto const end{directionin * shape.end.x + normalin * shape.end.y};
auto const along{cross(directionout, end) / cross(directionout, directionin)};
Tangentin = std::max(0.0, along);
Tangentout = std::max(0.0, glm::dot(end - directionin * along, directionout));
};
fit.tangents(fit.radius, fit.tangent_in, fit.tangent_out);
continue;
}
if (vertex.spirals)
{
// the lengths follow from the radius, so a smaller one makes the whole curve smaller in proportion
fit.spirals = true;
fit.deflection = deflection;
fit.transition_in = fit.transition_out = spirals_length(Design.shape, deflection, fit.radius);
fit.tangents = [&, directionin, directionout, side](double const Radius, double &Tangentin, double &Tangentout) {
auto const length{spirals_length(Design.shape, deflection, Radius)};
curve_tangents(Design.shape, length, length, Radius, directionin, directionout, side, Tangentin, Tangentout);
};
fit.tangents(fit.radius, fit.tangent_in, fit.tangent_out);
continue;
}
auto const angles = [&](double const Factor) {
return transition_point(Design.shape, fit.transition_in * Factor, fit.radius, 1.0).angle + transition_point(Design.shape, fit.transition_out * Factor, fit.radius, 1.0).angle;
};
if (angles(1.0) > deflection)
{
double low{0.0};
double high{1.0};
for (int i = 0; i < 50; ++i)
{
auto const middle{(low + high) * 0.5};
(angles(middle) > deflection ? high : low) = middle;
}
r.warnings.emplace_back(format(STR_C("Vertex %zu: transition curves shortened to %.1f / %.1f m to fit the deflection angle"), k + 1, fit.transition_in * low, fit.transition_out * low));
fit.transition_in *= low;
fit.transition_out *= low;
}
fit.deflection = deflection;
fit.tangents = [&, k, directionin, directionout, side](double const Radius, double &Tangentin, double &Tangentout) {
curve_tangents(Design.shape, fits[k].transition_in, fits[k].transition_out, Radius, directionin, directionout, side, Tangentin, Tangentout);
};
fit.tangents(fit.radius, fit.tangent_in, fit.tangent_out);
}
std::vector<double> scales(count, 1.0);
for (std::size_t leg = 0; leg + 1 < polygon.size(); ++leg)
{
auto const available{glm::distance(polygon[leg], polygon[leg + 1]) + (leg == 0 ? startreserve : 0.0) + (leg == count ? endreserve : 0.0)};
auto const required{(leg > 0 ? fits[leg - 1].tangent_out : 0.0) + (leg < count ? fits[leg].tangent_in : 0.0)};
if (required <= available + 1e-6)
continue;
auto const scale{available / required * (1.0 - 1e-9)};
if (leg > 0)
scales[leg - 1] = std::min(scales[leg - 1], scale);
if (leg < count)
scales[leg] = std::min(scales[leg], scale);
}
for (std::size_t k = 0; k < count; ++k)
{
if (scales[k] >= 1.0 || fits[k].tangents == nullptr)
continue;
auto &fit{fits[k]};
auto const budgetin{fit.tangent_in * scales[k]};
auto const budgetout{fit.tangent_out * scales[k]};
auto const fits_budget = [&](double const Radius) {
if (transition_point(Design.shape, fit.transition_in, Radius, 1.0).angle + transition_point(Design.shape, fit.transition_out, Radius, 1.0).angle > fit.deflection)
return false;
double tangentin;
double tangentout;
fit.tangents(Radius, tangentin, tangentout);
return tangentin <= budgetin && tangentout <= budgetout;
};
double low{1e-3};
double high{fit.radius};
for (int i = 0; i < 60; ++i)
{
auto const middle{(low + high) * 0.5};
(transition_point(Design.shape, fit.transition_in, middle, 1.0).angle + transition_point(Design.shape, fit.transition_out, middle, 1.0).angle > fit.deflection ? low : high) = middle;
}
low = high * (1.0 + 1e-9);
high = fit.radius;
if (false == fit.compound && false == fit.spirals && fits_budget(low))
{
for (int i = 0; i < 60; ++i)
{
auto const middle{(low + high) * 0.5};
(fits_budget(middle) ? low : high) = middle;
}
fit.radius = low;
}
else
{
fit.radius *= scales[k];
fit.arcs = scaled(fit.arcs, scales[k]);
fit.transition_in *= scales[k];
fit.transition_out *= scales[k];
}
r.warnings.emplace_back(format(STR_C("Vertex %zu: R reduced from %.1f to %.1f m, transitions to %.1f / %.1f m, to fit between the ends"), k + 1, Design.vertices[k].radius, fit.radius, fit.transition_in, fit.transition_out));
r.radius_warnings.push_back(r.warnings.back());
}
for (std::size_t k = 0; k < count; ++k)
{
auto &fit{fits[k]};
auto const &vertex{Design.vertices[k]};
fit.cant = fit.spirals ? 0.0 : std::max(0.0, vertex.cant);
if (fit.compound || fit.spirals || fit.deflection < 1e-7 || fit.transition_in + fit.transition_out <= 0.0)
continue;
auto const angles = [&](double const Factor) {
return transition_point(Design.shape, fit.transition_in * Factor, fit.radius, 1.0).angle + transition_point(Design.shape, fit.transition_out * Factor, fit.radius, 1.0).angle;
};
auto const minimum{std::min(fit.radius * fit.deflection, std::max(Design.norms.arc_min, Design.norms.arc_min_factor * Design.speed))};
if (fit.radius * (fit.deflection - angles(1.0)) < minimum - 1e-6)
{
auto const target{fit.deflection - minimum / fit.radius};
double low{0.0};
double high{1.0};
for (int i = 0; i < 50; ++i)
{
auto const middle{(low + high) * 0.5};
(angles(middle) > target ? high : low) = middle;
}
fit.transition_in *= low;
fit.transition_out *= low;
r.warnings.emplace_back(format(STR_C("Vertex %zu: transition curves shortened to %.1f / %.1f m, the circular arc keeps %.0f m"), k + 1, fit.transition_in, fit.transition_out, minimum));
}
if (fit.transition_in < 1.0)
fit.transition_in = 0.0;
if (fit.transition_out < 1.0)
fit.transition_out = 0.0;
auto const shortest{std::min(fit.transition_in, fit.transition_out)};
if (fit.cant <= 0.0 || (fit.transition_in >= vertex.transition_in - 1e-6 && fit.transition_out >= vertex.transition_out - 1e-6))
continue;
auto const speed{std::max(1.0, Design.speed)};
auto const ramp{std::min(shortest * 1000.0 / (Design.norms.ramp_factor * speed), Design.norms.cant_rate * 3.6 * shortest / speed)};
if (ramp >= fit.cant)
continue;
fit.cant = ramp < 5.0 ? 0.0 : std::floor(ramp / 5.0) * 5.0;
r.warnings.emplace_back(format(STR_C("Vertex %zu: cant lowered to %.0f mm, the shortened transition curves can't ramp more"), k + 1, fit.cant));
}
std::vector<double> vertexchainage(count, 0.0);
glm::dvec2 cursor{start};
for (std::size_t k = 0; k < count; ++k)
{
auto const &vertex{Design.vertices[k]};
auto const &fit{fits[k]};
auto const &position{polygon[k + 1]};
auto const directionin{glm::normalize(polygon[k + 1] - polygon[k])};
auto const directionout{glm::normalize(polygon[k + 2] - polygon[k + 1])};
auto const turn{cross(directionin, directionout)};
auto const geometric{std::atan2(std::abs(turn), glm::dot(directionin, directionout))};
auto const reverse{Design.vertices[k].reverse_turn};
auto const deflection{reverse ? 2.0 * kPi - geometric : geometric};
int const side{(turn > 0.0) != reverse ? 1 : -1};
auto const lead{glm::dot(position - cursor, directionin)};
if (deflection < 1e-7)
{
if (lead < -1e-4)
r.errors.emplace_back(format(STR_C("Vertex %zu lies inside the curve of the previous vertex"), k + 1));
push_straight(cursor, directionin, lead);
vertexchainage[k] = chainage;
cursor = position;
continue;
}
if (geometric > kPi - 1e-3 || (reverse && geometric < 1e-3))
{
r.errors.emplace_back(format(STR_C("Vertex %zu: a turn of about 180 or 360 degrees needs one more vertex"), k + 1));
return r;
}
if (fit.compound)
{
auto const cants{arc_cants(fit.arcs, fit.cant)};
auto const shape{build_compound(deflection, fit.arcs, fit.transition_in, fit.transition_out, cants)};
if (shape.shortened < 1.0)
r.warnings.emplace_back(format(STR_C("Vertex %zu: transition curves shortened to fit the deflection angle"), k + 1));
auto const normalin{perpendicular(directionin) * static_cast<double>(side)};
auto const endoffset{directionin * shape.end.x + normalin * shape.end.y};
auto const along{cross(directionout, endoffset) / cross(directionout, directionin)};
auto const curvestart{position - directionin * along};
auto const straight{glm::dot(curvestart - cursor, directionin)};
if (k == 0)
starts_early(straight);
else if (straight < -1e-4)
r.errors.emplace_back(format(STR_C("Curves of vertices %zu and %zu overlap by %.2f m"), k, k + 1, -straight));
push_straight(cursor, directionin, straight);
auto const startchainage{chainage};
glm::dvec2 origin{curvestart};
glm::dvec2 heading{directionin};
auto sharpest{fit.radius};
for (auto const &arc : fit.arcs)
sharpest = std::min(sharpest, arc.radius);
for (auto const &segment : shape.segments)
{
element piece;
piece.kind = element_kind::spiral;
piece.vertex = static_cast<int>(k);
piece.turn = side;
piece.cant = std::max(segment[3], segment[4]);
piece.cant_start = segment[3];
piece.cant_end = segment[4];
piece.curvature_start = segment[0];
piece.curvature_end = segment[1];
piece.radius = std::max(segment[0], segment[1]) > 0.0 ? 1.0 / std::max(segment[0], segment[1]) : 0.0;
piece.length = segment[2];
piece.origin = origin;
piece.direction = heading;
piece.normal = perpendicular(heading) * static_cast<double>(side);
push(piece);
auto const local{spiral_point(segment[0], segment[1], segment[2], segment[2])};
origin += heading * local.x + piece.normal * local.y;
heading = turned(heading, side * local.angle);
}
vertexchainage[k] = (startchainage + chainage) * 0.5;
cursor = origin;
curve_report report;
report.vertex = static_cast<int>(k);
report.radius = fit.radius;
for (auto const &arc : fit.arcs)
report.radii.push_back(arc.radius);
report.transition_in = fit.transition_in * shape.shortened;
report.transition_out = fit.transition_out * shape.shortened;
report.deflection = deflection;
report.arc_length = chainage - startchainage;
report.tangent_in = glm::distance(position, curvestart);
report.tangent_out = glm::distance(position, cursor);
report.cants = cants;
report.unbalanced = -std::numeric_limits<double>::max();
for (std::size_t i = 0; i < cants.size(); ++i)
{
report.unbalanced = std::max(report.unbalanced, unbalanced_acceleration(Design.speed, fit.arcs[i].radius, cants[i], Design.norms));
report.cant = std::max(report.cant, cants[i]);
}
report.recommended = recommend(Design.speed, sharpest, Design.norms);
r.curves.push_back(report);
auto const &norms{Design.norms};
if (false == norms.checked)
continue;
if (sharpest < report.recommended.radius_min - 1e-6)
r.warnings.emplace_back(format(STR_C("Vertex %zu: R %.0f m is below the minimum %.0f m for %.0f km/h"), k + 1, sharpest, report.recommended.radius_min, Design.speed));
if (report.unbalanced > norms.unbalanced + 1e-6)
r.warnings.emplace_back(format(STR_C("Vertex %zu: unbalanced acceleration %.2f m/s2 exceeds %.2f m/s2"), k + 1, report.unbalanced, norms.unbalanced));
if (report.cant > norms.cant_max + 1e-6)
r.warnings.emplace_back(format(STR_C("Vertex %zu: cant %.0f mm exceeds %.0f mm"), k + 1, report.cant, norms.cant_max));
// each transition long enough for the change of the curvature and of the cant along it, at the speed the sharpest arc
// allows, as the transitions of the norms are set between the arcs
auto const allowed{std::max(1.0, std::min(Design.speed, speed_limit(sharpest, norms)))};
auto const requiredin{transition_length(allowed, fit.arcs.front().radius, cants.front(), norms)};
auto const requiredout{transition_length(allowed, fit.arcs.back().radius, cants.back(), norms)};
if (report.transition_in + 1e-6 < requiredin || report.transition_out + 1e-6 < requiredout)
r.warnings.emplace_back(format(STR_C("Vertex %zu: transition curves shorter than the required %.0f m"), k + 1, std::max(requiredin, requiredout)));
for (std::size_t i = 1; i < fit.arcs.size(); ++i)
{
auto const required{transition_between(allowed, 1.0 / fit.arcs[i - 1].radius, cants[i - 1], 1.0 / fit.arcs[i].radius, cants[i], norms)};
if (fit.arcs[i].transition * shape.shortened + 1e-6 < required)
r.warnings.emplace_back(format(STR_C("Vertex %zu: the transition curve between R %.0f and R %.0f m shorter than the required %.0f m"), k + 1, fit.arcs[i - 1].radius, fit.arcs[i].radius, required));
}
continue;
}
auto const radius{fit.radius};
auto const lengthin{fit.transition_in};
auto const lengthout{fit.transition_out};
auto const endin{transition_point(Design.shape, lengthin, radius, 1.0)};
auto const endout{transition_point(Design.shape, lengthout, radius, 1.0)};
auto const shiftin{endin.y - radius * (1.0 - std::cos(endin.angle))};
auto const shiftout{endout.y - radius * (1.0 - std::cos(endout.angle))};
auto const middlein{endin.x - radius * std::sin(endin.angle)};
auto const middleout{endout.x - radius * std::sin(endout.angle)};
auto const normalin{perpendicular(directionin) * static_cast<double>(side)};
auto const normalout{perpendicular(directionout) * static_cast<double>(side)};
auto const a{radius + shiftin};
auto const b{radius + shiftout};
auto const determinant{cross(normalin, normalout)};
glm::dvec2 const centre{position + glm::dvec2{(a * normalout.y - normalin.y * b) / determinant, (normalin.x * b - a * normalout.x) / determinant}};
auto const curvestart{centre - normalin * a - directionin * middlein};
auto const curveend{centre - normalout * b + directionout * middleout};
auto const arcangle{deflection - endin.angle - endout.angle};
if (arcangle < -1e-9 && false == fit.spirals)
{
r.errors.emplace_back(format(STR_C("Vertex %zu: transition curves are too long for the deflection angle"), k + 1));
return r;
}
auto const straight{glm::dot(curvestart - cursor, directionin)};
if (k == 0)
starts_early(straight);
else if (straight < -1e-4)
r.errors.emplace_back(format(STR_C("Curves of vertices %zu and %zu overlap by %.2f m"), k, k + 1, -straight));
push_straight(cursor, directionin, straight);
element curve;
curve.vertex = static_cast<int>(k);
curve.radius = radius;
curve.turn = side;
curve.cant = fit.cant;
if (lengthin > 0.0)
{
curve.kind = element_kind::transition_in;
curve.length = lengthin;
curve.transition = curve.length;
curve.origin = curvestart;
curve.direction = directionin;
curve.normal = normalin;
push(curve);
}
auto const arcstart{curvestart + directionin * endin.x + normalin * endin.y};
curve.kind = element_kind::arc;
curve.transition = 0.0;
curve.length = radius * std::max(0.0, arcangle);
curve.origin = centre;
curve.normal = arcstart - centre;
curve.direction = turned(directionin, side * endin.angle);
vertexchainage[k] = chainage + curve.length * 0.5;
if (curve.length > 1e-6)
push(curve);
if (lengthout > 0.0)
{
curve.kind = element_kind::transition_out;
curve.length = lengthout;
curve.transition = curve.length;
curve.origin = curveend;
curve.direction = -directionout;
curve.normal = normalout;
push(curve);
}
cursor = curveend;
curve_report report;
report.vertex = static_cast<int>(k);
report.radius = radius;
report.transition_in = lengthin;
report.transition_out = lengthout;
report.deflection = deflection;
report.arc_length = radius * std::max(0.0, arcangle);
report.tangent_in = glm::distance(position, curvestart);
report.tangent_out = glm::distance(position, curveend);
report.unbalanced = unbalanced_acceleration(Design.speed, radius, curve.cant, Design.norms);
report.cant = curve.cant;
auto const speed{Design.speed / 3.6};
if (lengthin > 0.0)
{
report.cant_rate_in = curve.cant * speed / lengthin;
report.jerk_in = std::max(0.0, report.unbalanced) * speed / lengthin;
}
if (lengthout > 0.0)
{
report.cant_rate_out = curve.cant * speed / lengthout;
report.jerk_out = std::max(0.0, report.unbalanced) * speed / lengthout;
}
report.recommended = recommend(Design.speed, radius, Design.norms);
report.spirals = fit.spirals;
r.curves.push_back(report);
auto const &norms{Design.norms};
if (false == norms.checked)
continue;
if (fit.spirals)
{
// with no cant all the acceleration across is unbalanced
if (report.unbalanced > norms.unbalanced + 1e-6)
r.warnings.emplace_back(format(STR_C("Vertex %zu: the transition curves meet at R %.0f m, with no cant at least %.0f m for %.0f km/h"), k + 1, radius, Design.speed * Design.speed / (12.96 * norms.unbalanced), Design.speed));
}
else
{
if (radius < report.recommended.radius_min - 1e-6)
r.warnings.emplace_back(format(STR_C("Vertex %zu: R %.0f m is below the minimum %.0f m for %.0f km/h"), k + 1, radius, report.recommended.radius_min, Design.speed));
if (report.unbalanced > norms.unbalanced + 1e-6)
r.warnings.emplace_back(format(STR_C("Vertex %zu: unbalanced acceleration %.2f m/s2 exceeds %.2f m/s2 (cant at least %.0f mm)"), k + 1, report.unbalanced, norms.unbalanced, report.recommended.cant_min));
}
if (curve.cant > norms.cant_max + 1e-6)
r.warnings.emplace_back(format(STR_C("Vertex %zu: cant %.0f mm exceeds %.0f mm"), k + 1, curve.cant, norms.cant_max));
auto const required{transition_length(Design.speed, radius, curve.cant, norms)};
if (lengthin + 1e-6 < required || lengthout + 1e-6 < required)
r.warnings.emplace_back(format(STR_C("Vertex %zu: transition curves shorter than the required %.0f m"), k + 1, required));
if (curve.cant > 0.0 && (lengthin <= 0.0 || lengthout <= 0.0))
r.warnings.emplace_back(format(STR_C("Vertex %zu: cant without a transition curve to ramp it"), k + 1));
}
auto const lastdirection{glm::normalize(polygon.back() - polygon[polygon.size() - 2])};
auto const tail{glm::dot(end - cursor, lastdirection)};
if (tail < 0.0)
r.end_extension = -tail;
if (tail < -1e-4 && -tail > endreserve + 1e-4)
{
if (endreserve > 0.0)
r.errors.emplace_back(format(STR_C("Vertex %zu: the curve ends %.2f m past the adjoining straight"), count, -tail - endreserve));
else
r.errors.emplace_back(format(STR_C("Vertex %zu: the curve ends %.2f m past the fixed end"), count, -tail));
}
push_straight(cursor, lastdirection, tail);
r.length = chainage;
if (Design.norms.checked && Design.norms.tangent_min_time > 0.0)
{
auto const minimum{Design.norms.tangent_min_time * Design.speed / 3.6};
for (std::size_t i = 1; i + 1 < r.elements.size(); ++i)
{
auto const &e{r.elements[i]};
if (e.kind == element_kind::straight && e.length < minimum && r.elements[i - 1].kind != element_kind::straight && r.elements[i + 1].kind != element_kind::straight)
r.warnings.emplace_back(format(STR_C("Straight between curves is %.1f m long, less than %.0f m"), e.length, minimum));
}
}
r.vertex_chainages = vertexchainage;
r.profile.push_back({0.0, Design.start.y, 0.0, 0.0});
auto const radius{std::max(Design.norms.vertical_min, Design.norms.vertical_factor * Design.speed * Design.speed)};
for (std::size_t k = 0; k < count; ++k)
{
auto const elevation{Design.start.y + (Design.end.y - Design.start.y) * vertexchainage[k] / std::max(r.length, 1e-6)};
r.vertex_elevations.push_back(elevation);
if (vertexchainage[k] <= r.profile.back().chainage + 1e-3)
continue;
r.profile.push_back({vertexchainage[k], elevation, radius, 0.0});
}
r.profile.push_back({r.length, Design.end.y, 0.0, 0.0});
for (std::size_t i = 1; i + 1 < r.profile.size(); ++i)
{
auto &point{r.profile[i]};
auto const gradebefore{(point.elevation - r.profile[i - 1].elevation) / (point.chainage - r.profile[i - 1].chainage)};
auto const gradeafter{(r.profile[i + 1].elevation - point.elevation) / (r.profile[i + 1].chainage - point.chainage)};
point.tangent = point.radius * std::abs(gradeafter - gradebefore) / 2.0;
}
if (Design.start_radius > 0.0 && Design.start_radius < 10000.0)
r.warnings.emplace_back(format(STR_C("The adjoining track at the start is curved (R %.0f m), the curvature changes abruptly"), Design.start_radius));
if (Design.end_radius > 0.0 && Design.end_radius < 10000.0)
r.warnings.emplace_back(format(STR_C("The adjoining track at the end is curved (R %.0f m), the curvature changes abruptly"), Design.end_radius));
make_cants(Design, r);
r.valid = r.errors.empty() && r.length > 1e-3;
return r;
}
glm::dvec3 along_end(glm::dvec3 const &Point, glm::dvec2 const &Direction, double const Grade, double const Distance)
{
auto const direction{glm::normalize(Direction)};
return {Point.x + direction.x * Distance, Point.y + Grade * Distance, Point.z + direction.y * Distance};
}
} // namespace
result compute(design const &Design)
{
auto first{fit_between(Design)};
auto const startextension{first.start_extension > 1e-4 ? first.start_extension : 0.0};
auto const endextension{first.end_extension > 1e-4 ? first.end_extension : 0.0};
if (false == first.errors.empty() || (startextension == 0.0 && endextension == 0.0))
{
first.start_extension = 0.0;
first.end_extension = 0.0;
return first;
}
auto extended{Design};
extended.start = along_end(Design.start, Design.start_direction, Design.start_grade, -startextension);
extended.end = along_end(Design.end, Design.end_direction, Design.end_grade, endextension);
extended.start_reserve = 0.0;
extended.end_reserve = 0.0;
if (extended.vertices.size() >= 2)
{
extended.vertices.front().offset += startextension;
extended.vertices.back().offset += endextension;
}
auto r{fit_between(extended)};
r.start_extension = startextension;
r.end_extension = endextension;
return r;
}
double speed_limit(double const Radius, limits const &Limits)
{
if (Radius <= 0.0)
return 0.0;
auto const factor{Limits.gauge / (kGravity * 12.96)};
auto const allowance{Limits.gauge * Limits.unbalanced / kGravity};
return std::sqrt(Radius * (Limits.cant_max + allowance) / factor);
}
result compute_recommended(design &Design, bool const Transitions)
{
for (auto &vertex : Design.vertices)
{
vertex.cant = 0.0;
vertex.transition_in = 0.0;
vertex.transition_out = 0.0;
}
auto r{compute(Design)};
if (false == Transitions || false == r.valid)
return r;
auto const speed{Design.speed};
for (int i = 0; i < 3; ++i)
{
bool accepted{false};
for (auto const share : {1.0, 0.75, 0.5, 0.35, 0.25})
{
auto attempt{Design};
auto slowest{speed};
for (auto const &curve : r.curves)
{
if (curve.vertex < 0 || curve.vertex >= static_cast<int>(attempt.vertices.size()) || attempt.vertices[curve.vertex].spirals)
continue;
auto const allowed{std::max(1.0, std::min(speed, speed_limit(curve.radius, attempt.norms)))};
auto const recommended{recommend(allowed, curve.radius, attempt.norms)};
auto const length{recommended.transition * share};
auto &vertex{attempt.vertices[curve.vertex]};
vertex.transition_out = length;
vertex.transition_in = vertex.transition_out;
vertex.cant = std::min({recommended.cant, length * 1000.0 / (attempt.norms.ramp_factor * allowed), attempt.norms.cant_rate * 3.6 * length / allowed});
vertex.cant = vertex.cant < 5.0 ? 0.0 : std::floor(vertex.cant / 5.0) * 5.0;
slowest = std::min(slowest, allowed);
}
attempt.speed = slowest;
auto next{compute(attempt)};
if (false == next.valid)
continue;
auto const kept{std::all_of(next.curves.begin(), next.curves.end(), [&](curve_report const &Curve) {
auto const &vertex{attempt.vertices[Curve.vertex]};
return Curve.transition_in >= 0.95 * vertex.transition_in && Curve.transition_out >= 0.95 * vertex.transition_out;
})};
if (false == kept && share > 0.25)
continue;
Design = attempt;
r = next;
accepted = true;
break;
}
if (false == accepted)
break;
}
return r;
}
namespace
{
double turning(result const &Result)
{
double total{0.0};
for (auto const &e : Result.elements)
{
if (e.radius <= 0.0)
continue;
if (e.kind == element_kind::arc)
total += e.length / e.radius;
else if (e.kind == element_kind::transition_in || e.kind == element_kind::transition_out)
total += e.length / (2.0 * e.radius);
}
return total;
}
void spread_rolls(std::vector<segment_data> &Pieces, double const Start, double const End)
{
auto const count{static_cast<double>(Pieces.size())};
for (std::size_t i = 0; i < Pieces.size(); ++i)
Pieces[i].rolls = {static_cast<float>(Start + (End - Start) * i / count), static_cast<float>(Start + (End - Start) * (i + 1) / count)};
}
} // namespace
namespace
{
struct join_frame
{
join_request const &request;
glm::dvec2 start_direction;
glm::dvec2 end_direction;
glm::dvec2 chord;
double distance;
double tolerance;
double heading;
double rise;
};
std::optional<join_result> straight_join(join_frame const &Frame)
{
auto const &a{Frame.request.start};
auto const &b{Frame.request.end};
auto const kink{std::max(std::abs(geometry::signed_angle(Frame.start_direction, Frame.chord)), std::abs(geometry::signed_angle(Frame.chord, Frame.end_direction)))};
if (kink > Frame.tolerance)
return std::nullopt;
join_result straight;
straight.kind = join_kind::straight;
straight.kink = kink;
segment_data path;
path.points[segment_data::point::start] = a.position;
path.points[segment_data::point::end] = b.position;
straight.pieces = {path};
spread_rolls(straight.pieces, a.roll, b.roll);
straight.length = glm::distance(a.position, b.position);
return straight;
}
std::optional<join_result> arc_join(join_frame const &Frame)
{
auto const &a{Frame.request.start};
auto const &b{Frame.request.end};
auto const lateral{cross(Frame.start_direction, Frame.chord)};
if (std::abs(lateral) <= 1e-6)
return std::nullopt;
auto const angle{2.0 * std::atan2(std::abs(lateral), glm::dot(Frame.start_direction, Frame.chord))};
int const side{lateral > 0.0 ? 1 : -1};
auto const radius{Frame.distance * Frame.distance / (2.0 * std::abs(lateral))};
auto const kink{std::abs(geometry::signed_angle(turned(Frame.start_direction, side * angle), Frame.end_direction))};
if (angle >= kPi - 1e-3 || kink > Frame.tolerance)
return std::nullopt;
join_result arc;
arc.kind = join_kind::arc;
arc.kink = kink;
arc.pieces = geometry::arc_pieces(a.position, Frame.start_direction, Frame.rise / (radius * angle), radius, angle, side);
arc.pieces.back().points[segment_data::point::end] = b.position;
spread_rolls(arc.pieces, a.roll, b.roll);
arc.radius = radius;
arc.length = radius * angle;
return arc;
}
design join_design(join_frame const &Frame, join_kind const Kind)
{
auto const &request{Frame.request};
design shape;
shape.norms = request.norms;
shape.shape = request.shape;
shape.speed = request.speed;
shape.arc_piece_angle = 90.0;
shape.start = request.start.position;
shape.end = request.end.position;
shape.start_direction = Frame.start_direction;
shape.end_direction = Frame.end_direction;
shape.start_grade = Frame.rise / Frame.distance;
shape.end_grade = shape.start_grade;
vertex corner;
corner.radius = request.radius > 0.0 ? request.radius : 100000.0;
if (Kind == join_kind::curve)
shape.vertices = {corner};
else
{
corner.offset = Frame.distance / 4.0;
shape.vertices = {corner, corner};
}
return shape;
}
std::optional<join_result> fitted_join(join_frame const &Frame, join_kind const Kind)
{
auto shape{join_design(Frame, Kind)};
auto const r{compute_recommended(shape, Frame.request.transitions)};
auto const sharp{std::any_of(r.curves.begin(), r.curves.end(), [](curve_report const &Report) { return Report.radius < 10.0; })};
if (false == r.valid || sharp || turning(r) > Frame.heading + kPi + 1e-6 || r.length > 3.0 * Frame.distance + 1.0)
return std::nullopt;
join_result curve;
curve.kind = Kind;
curve.pieces = pieces(r, shape, 0);
if (curve.pieces.empty())
return curve;
curve.pieces.front().rolls[0] = static_cast<float>(Frame.request.start.roll);
curve.pieces.back().rolls[1] = static_cast<float>(Frame.request.end.roll);
curve.length = r.length;
curve.speed = shape.speed;
curve.warnings = r.warnings;
curve.radius_warnings = r.radius_warnings;
curve.radius = r.curves.empty() ? 0.0 : std::numeric_limits<double>::max();
for (auto const &report : r.curves)
{
curve.radius = std::min(curve.radius, report.radius);
curve.transition = std::max({curve.transition, report.transition_in, report.transition_out});
}
for (auto const &e : r.elements)
curve.cant = std::max(curve.cant, e.cant);
return curve;
}
std::optional<join_result> curve_join(join_frame const &Frame)
{
design probe;
probe.start = Frame.request.start.position;
probe.end = Frame.request.end.position;
probe.start_direction = Frame.start_direction;
probe.end_direction = Frame.end_direction;
glm::dvec2 intersection;
return fitted_join(Frame, tangent_intersection(probe, intersection) ? join_kind::curve : join_kind::reverse_curve);
}
join_result bezier_join(join_frame const &Frame)
{
auto const &a{Frame.request.start};
auto const &b{Frame.request.end};
join_result fallback;
fallback.kind = join_kind::bezier;
auto const grade{Frame.rise / Frame.distance};
auto const handle{Frame.distance / 3.0};
segment_data path;
path.points[segment_data::point::start] = a.position;
path.points[segment_data::point::end] = b.position;
path.points[segment_data::point::control1] = glm::dvec3{Frame.start_direction.x, grade, Frame.start_direction.y} * handle;
path.points[segment_data::point::control2] = -glm::dvec3{Frame.end_direction.x, grade, Frame.end_direction.y} * handle;
fallback.pieces = {path};
spread_rolls(fallback.pieces, a.roll, b.roll);
fallback.length = Frame.distance;
return fallback;
}
} // namespace
join_result join(join_request const &Request)
{
auto const &a{Request.start};
auto const &b{Request.end};
auto const chord{plan_of(b.position) - plan_of(a.position)};
auto const distance{glm::length(chord)};
if (distance < 0.05 || glm::length(a.direction) < 1e-9 || glm::length(b.direction) < 1e-9)
return {};
auto const startdirection{glm::normalize(a.direction)};
auto const enddirection{glm::normalize(b.direction)};
join_frame const frame{Request, startdirection, enddirection, chord, distance, glm::radians(Request.kink), std::abs(geometry::signed_angle(startdirection, enddirection)), b.position.y - a.position.y};
std::vector<join_result> candidates;
std::vector<join_result> kinked;
for (auto const &candidate : {straight_join(frame), arc_join(frame), curve_join(frame)})
if (candidate.has_value())
(candidate->kink > 0.1 * frame.tolerance ? kinked : candidates).push_back(*candidate);
candidates.insert(candidates.end(), kinked.begin(), kinked.end());
if (candidates.empty())
candidates.push_back(bezier_join(frame));
auto const count{static_cast<int>(candidates.size())};
auto chosen{candidates[((Request.variant % count) + count) % count]};
chosen.variants = count;
if (chosen.speed <= 0.0)
chosen.speed = Request.speed;
return chosen;
}
namespace
{
// elements laid one after another from a point in a direction, put together the way the alignment computes them; the cant along
// them is kept apart, a transition between two canted arcs ramps it from the one to the other
struct element_chain
{
result shape;
glm::dvec2 position{0.0};
glm::dvec2 direction{0.0, 1.0};
std::vector<std::array<double, 2>> cants; // chainage, cant
};
void chain_push(element_chain &Chain, element Element, double const Cant0, double const Cant1)
{
Element.chainage = Chain.shape.length;
Chain.shape.length += Element.length;
Chain.shape.elements.push_back(Element);
Chain.cants.push_back({Element.chainage, Cant0});
Chain.cants.push_back({Chain.shape.length, Cant1});
}
void chain_straight(element_chain &Chain, double const Length)
{
if (Length < 1e-6)
return;
element straight;
straight.kind = element_kind::straight;
straight.origin = Chain.position;
straight.direction = Chain.direction;
straight.length = Length;
Chain.position += Chain.direction * Length;
chain_push(Chain, straight, 0.0, 0.0);
}
void chain_arc(element_chain &Chain, double const Curvature, double const Length, double const Cant)
{
if (Length < 1e-6)
return;
if (std::abs(Curvature) < 1e-9)
{
chain_straight(Chain, Length);
return;
}
element arc;
arc.kind = element_kind::arc;
arc.turn = Curvature > 0.0 ? 1 : -1;
arc.radius = 1.0 / std::abs(Curvature);
arc.length = Length;
arc.origin = Chain.position + perpendicular(Chain.direction) / Curvature;
arc.normal = Chain.position - arc.origin;
arc.direction = Chain.direction;
auto const angle{arc.turn * Length / arc.radius};
Chain.position = arc.origin + turned(arc.normal, angle);
Chain.direction = turned(Chain.direction, angle);
chain_push(Chain, arc, Cant, Cant);
}
// a curvature going on through 0 turns the curve the other way: it's kept signed, the element turning to the left
void chain_spiral(element_chain &Chain, double const From, double const To, double const Length, double const Cant0, double const Cant1)
{
if (Length < 1e-6)
return;
int const turn{(From * To < 0.0 || From + To >= 0.0) ? 1 : -1};
element spiral;
spiral.kind = element_kind::spiral;
spiral.turn = turn;
spiral.curvature_start = From * turn;
spiral.curvature_end = To * turn;
auto const peak{std::max(std::abs(From), std::abs(To))};
spiral.radius = peak > 1e-12 ? 1.0 / peak : 0.0;
spiral.length = Length;
spiral.origin = Chain.position;
spiral.direction = Chain.direction;
spiral.normal = perpendicular(Chain.direction) * static_cast<double>(turn);
auto const local{spiral_point(spiral.curvature_start, spiral.curvature_end, Length, Length)};
Chain.position = spiral.origin + spiral.direction * local.x + spiral.normal * local.y;
Chain.direction = glm::normalize(spiral.direction * std::cos(local.angle) + spiral.normal * std::sin(local.angle));
chain_push(Chain, spiral, Cant0, Cant1);
}
lay_result chain_pieces(element_chain &Chain, lay_request const &Request)
{
auto &shape{Chain.shape};
shape.gauge = Request.norms.gauge;
shape.start = Request.start.position;
shape.end = {Chain.position.x, Request.target.y, Chain.position.y};
shape.profile = {{0.0, Request.start.position.y, 0.0, 0.0}, {shape.length, Request.target.y, 0.0, 0.0}};
shape.valid = shape.length > 1e-3;
design style;
style.norms = Request.norms;
lay_result result;
result.length = shape.length;
auto const count{std::max(minimum_pieces(shape, style), static_cast<std::size_t>(std::ceil(shape.length / std::max(5.0, Request.piece_length) - 1e-9)))};
result.pieces = pieces(shape, style, count);
auto const cant_at = [&](double const Chainage) {
auto const &cants{Chain.cants};
for (std::size_t i = 0; i + 1 < cants.size(); i += 2)
if (Chainage <= cants[i + 1][0] + 1e-6)
{
auto const span{cants[i + 1][0] - cants[i][0]};
auto const t{span > 1e-9 ? std::clamp((Chainage - cants[i][0]) / span, 0.0, 1.0) : 1.0};
return cants[i][1] + (cants[i + 1][1] - cants[i][1]) * t;
}
return cants.empty() ? 0.0 : cants.back()[1];
};
auto const roll = [&](double const Cant) { return static_cast<float>(glm::degrees(std::asin(std::clamp(Cant / Request.norms.gauge, -1.0, 1.0)))); };
double chainage{0.0};
for (auto &piece : result.pieces)
{
piece.rolls[0] = roll(cant_at(chainage));
chainage += geometry::bezier{piece}.plan_length();
piece.rolls[1] = roll(cant_at(chainage));
}
if (false == result.pieces.empty())
result.pieces.front().rolls[0] = static_cast<float>(Request.start.roll);
return result;
}
// a transition of the length given from the curvature of the start, as the position and the direction it ends with
struct spiral_end
{
double x{0.0};
double y{0.0};
double angle{0.0};
int turn{1};
spiral_end(double const From, double const To, double const Length)
{
turn = (From * To < 0.0 || From + To >= 0.0) ? 1 : -1;
auto const local{spiral_point(From * turn, To * turn, Length, Length)};
x = local.x;
y = local.y;
angle = local.angle;
}
void apply(glm::dvec2 &Position, glm::dvec2 &Direction) const
{
auto const normal{perpendicular(Direction) * static_cast<double>(turn)};
Position += Direction * x + normal * y;
Direction = glm::normalize(Direction * std::cos(angle) + normal * std::sin(angle));
}
};
void arc_end(glm::dvec2 &Position, glm::dvec2 &Direction, double const Curvature, double const Length)
{
auto const centre{Position + perpendicular(Direction) / Curvature};
auto const angle{Curvature * Length};
Position = centre + turned(Position - centre, angle);
Direction = turned(Direction, angle);
}
} // namespace
std::optional<lay_result> lay_on(lay_request const &Request)
{
auto const &norms{Request.norms};
if (glm::length(Request.start.direction) < 1e-9)
return std::nullopt;
glm::dvec2 const start{Request.start.position.x, Request.start.position.z};
auto const direction{glm::normalize(Request.start.direction)};
glm::dvec2 const target{Request.target.x, Request.target.z};
auto const offset{target - start};
auto const chord{glm::length(offset)};
auto const lateral{cross(direction, offset)};
if (chord < 0.5 || glm::dot(offset, direction) <= 0.0)
return std::nullopt;
auto const k0{Request.curvature};
auto const c0{norms.gauge * std::sin(glm::radians(Request.start.roll))};
if (Request.kind == lay_kind::straight && std::abs(k0) < 1e-9 && std::abs(c0) < 5.0)
return std::nullopt;
if (Request.kind == lay_kind::arc && std::abs(lateral) < 0.005 * chord + 0.05 && std::abs(k0) < 1e-9 && std::abs(c0) < 5.0)
return std::nullopt;
int const side{lateral > 0.0 ? 1 : -1};
// the speed the arc allows and its cant; a transition is ramped for the lower speed of its two ends
auto const allowed = [&](double const Curvature) { return std::abs(Curvature) < 1e-9 ? Request.speed : std::max(1.0, std::min(Request.speed, speed_limit(1.0 / std::abs(Curvature), norms))); };
auto const cant_of = [&](double const Curvature) { return std::abs(Curvature) < 1e-9 ? 0.0 : std::copysign(recommend(allowed(Curvature), 1.0 / std::abs(Curvature), norms).cant, Curvature); };
auto const slower = [&](double const From, double const To) { return std::min(allowed(From), allowed(To)); };
auto const between = [&](double const From, double const Fromcant, double const To, double const Tocant) {
if (std::abs(To - From) <= 0.05 * std::max(std::abs(From), std::abs(To)) && std::abs(Tocant - Fromcant) < 5.0)
return 0.0;
return transition_between(slower(From, To), From, Fromcant, To, Tocant, norms);
};
// the change of the cant a transition of the length ramps at the speed
auto const ramped = [&](double const Length, double const Speed) { return std::min(Length * 1000.0 / (norms.ramp_factor * Speed), norms.cant_rate * 3.6 * Length / Speed); };
// the transitions of the norms cut to the share, the cant of the arc lowered to what they can ramp; none where the cant of the
// end can't be ramped to one leaning into the arc
struct layout_plan
{
double out{0.0}; // out of the curvature of the end to the straight, before the straight of the layout
double entry{0.0};
double exit{0.0};
double cant{0.0};
double from{0.0}; // the curvature and the cant the entry starts from
double fromcant{0.0};
bool lowered{false};
};
auto const planned = [&](double const Curvature, bool const Lead, bool const Exit, double const Share) -> std::optional<layout_plan> {
layout_plan result;
auto const wanted{cant_of(Curvature)};
if (Lead)
{
result.out = between(k0, c0, 0.0, 0.0) * Share;
if (std::abs(c0) > ramped(result.out, slower(k0, 0.0)) + 1e-6)
return std::nullopt;
}
else
{
result.from = k0;
result.fromcant = c0;
}
result.entry = between(result.from, result.fromcant, Curvature, wanted) * Share;
result.exit = Exit ? between(Curvature, wanted, 0.0, 0.0) * Share : 0.0;
auto cant{wanted};
if (result.entry > 0.0)
{
auto const reach{ramped(result.entry, slower(result.from, Curvature))};
cant = std::clamp(cant, result.fromcant - reach, result.fromcant + reach);
}
else
cant = result.fromcant;
if (Exit)
{
auto const reach{result.exit > 0.0 ? ramped(result.exit, allowed(Curvature)) : 0.0};
cant = std::clamp(cant, -reach, reach);
}
if (cant * Curvature < -1e-9 || (result.entry > 0.0 && std::abs(cant - result.fromcant) > ramped(result.entry, slower(result.from, Curvature)) + 1e-6) || (result.entry <= 0.0 && std::abs(cant - result.fromcant) > 1e-6))
return std::nullopt;
result.cant = cant;
result.lowered = std::abs(cant) < std::abs(wanted) - 0.5;
return result;
};
auto const begin = [&]() {
element_chain chain;
chain.position = start;
chain.direction = direction;
return chain;
};
auto const made = [&](element_chain &Chain, double const Curvature, layout_plan const &Plan, double const Share) {
auto result{chain_pieces(Chain, Request)};
result.radius = std::abs(Curvature) > 1e-9 ? 1.0 / std::abs(Curvature) : 0.0;
result.cant = std::abs(Plan.cant);
result.entry = Plan.out > 0.0 ? Plan.out : Plan.entry;
result.exit = Plan.out > 0.0 ? Plan.entry : Plan.exit;
result.from_radius = std::abs(k0) > 1e-9 ? 1.0 / std::abs(k0) : 0.0;
result.compound = k0 * Curvature > 0.0 && Plan.out <= 0.0 && Plan.entry > 0.0;
if (Share < 1.0)
result.note = format(STR_C("The transition curves are cut to %.0f%% of what the norms ask for, there's no more room"), Share * 100.0);
if (Plan.lowered)
result.note += (result.note.empty() ? "" : "\n") + format(STR_C("The cant is lowered to %.0f mm, what the transition curves ramp"), std::abs(Plan.cant));
return result;
};
auto const arc_through = [&](double const Share) -> std::optional<lay_result> {
auto const reference{2.0 * lateral / (chord * chord)};
auto curvature{reference};
std::optional<layout_plan> plan;
// the transition and the cant follow the curvature found, which moves with them
for (int pass = 0; pass < 4; ++pass)
{
plan = planned(curvature, false, false, Share);
if (false == plan.has_value())
return std::nullopt;
// over the curvatures turning to the target, from flat to sharp, it goes from inside of the circle of the arc to outside
auto const outside = [&](double const Curvature) {
auto position{start};
auto heading{direction};
spiral_end{k0, Curvature, plan->entry}.apply(position, heading);
return glm::distance(target, position + perpendicular(heading) / Curvature) - 1.0 / std::abs(Curvature);
};
auto low{std::log(1e-6)};
auto high{std::log(0.1)};
if (outside(side * std::exp(low)) >= 0.0 || outside(side * std::exp(high)) <= 0.0)
return std::nullopt;
for (int i = 0; i < 60; ++i)
{
auto const middle{(low + high) * 0.5};
(outside(side * std::exp(middle)) < 0.0 ? low : high) = middle;
}
curvature = side * std::exp((low + high) * 0.5);
}
// a transition too long for the way to the target squeezes the arc after it, a shorter one is tried then
if (std::abs(curvature) > 1.3 * std::abs(reference) + 1e-5)
return std::nullopt;
auto chain{begin()};
chain_spiral(chain, k0, curvature, plan->entry, c0, plan->cant);
auto const centre{chain.position + perpendicular(chain.direction) / curvature};
auto const angle{geometry::signed_angle(chain.position - centre, target - centre) * side};
if (angle <= 1e-4 || angle >= kPi)
return std::nullopt;
chain_arc(chain, curvature, angle / std::abs(curvature), plan->cant);
return made(chain, curvature, *plan, Share);
};
auto const arc_straight = [&](double const Share) -> std::optional<lay_result> {
auto const curvature{side / std::max(1.0, Request.radius)};
auto const plan{planned(curvature, false, true, Share)};
if (false == plan.has_value())
return std::nullopt;
auto position{start};
auto heading{direction};
spiral_end{k0, curvature, plan->entry}.apply(position, heading);
spiral_end const out{curvature, 0.0, plan->exit};
// the arc grows until the straight after the transition out of it runs through the target
auto const miss = [&](double const Arc) {
auto at{position};
auto towards{heading};
arc_end(at, towards, curvature, Arc);
out.apply(at, towards);
return std::array<double, 2>{cross(towards, target - at) * side, glm::dot(towards, target - at)};
};
if (miss(0.0)[0] <= 0.0)
return std::nullopt;
auto const radius{1.0 / std::abs(curvature)};
auto const step{std::max(0.5, radius * glm::radians(2.0))};
double low{0.0};
double high{-1.0};
for (auto arc{step}; arc < 2.0 * kPi * radius; arc += step)
{
if (miss(arc)[0] <= 0.0)
{
high = arc;
break;
}
low = arc;
}
if (high < 0.0)
return std::nullopt;
for (int i = 0; i < 60; ++i)
{
auto const middle{(low + high) * 0.5};
(miss(middle)[0] > 0.0 ? low : high) = middle;
}
auto const arc{(low + high) * 0.5};
auto const straight{miss(arc)[1]};
if (straight < 0.0)
return std::nullopt;
auto chain{begin()};
chain_spiral(chain, k0, curvature, plan->entry, c0, plan->cant);
chain_arc(chain, curvature, arc, plan->cant);
chain_spiral(chain, curvature, 0.0, plan->exit, plan->cant, 0.0);
chain_straight(chain, straight);
return made(chain, curvature, *plan, Share);
};
auto const straight_arc = [&](double const Share) -> std::optional<lay_result> {
auto const curvature{side / std::max(1.0, Request.radius)};
auto const plan{planned(curvature, true, false, Share)};
if (false == plan.has_value())
return std::nullopt;
auto lead{begin()};
chain_spiral(lead, k0, 0.0, plan->out, c0, 0.0);
spiral_end const into{0.0, curvature, plan->entry};
// the end of the arc with no straight before it; the straight moves it along the lead, the arc sideways
auto const end = [&](double const Arc) {
auto at{lead.position};
auto towards{lead.direction};
into.apply(at, towards);
arc_end(at, towards, curvature, Arc);
return at;
};
auto const miss = [&](double const Arc) { return cross(lead.direction, target - end(Arc)) * side; };
auto const most{kPi / std::abs(curvature)};
if (miss(0.0) <= 0.0 || miss(most) > 0.0)
return std::nullopt;
double low{0.0};
double high{most};
for (int i = 0; i < 60; ++i)
{
auto const middle{(low + high) * 0.5};
(miss(middle) > 0.0 ? low : high) = middle;
}
auto const arc{(low + high) * 0.5};
auto const straight{glm::dot(lead.direction, target - end(arc))};
if (straight < 0.5)
return std::nullopt;
auto chain{lead};
chain_straight(chain, straight);
chain_spiral(chain, 0.0, curvature, plan->entry, 0.0, plan->cant);
chain_arc(chain, curvature, arc, plan->cant);
return made(chain, curvature, *plan, Share);
};
auto const straight = [&](double const Share) -> std::optional<lay_result> {
auto const plan{planned(0.0, true, false, Share)};
if (false == plan.has_value())
return std::nullopt;
auto chain{begin()};
chain_spiral(chain, k0, 0.0, plan->out, c0, 0.0);
auto const along{glm::dot(chain.direction, target - chain.position)};
if (along < 0.5)
return std::nullopt;
chain_straight(chain, along);
return made(chain, 0.0, *plan, Share);
};
auto const first = [](auto const &Layout) -> std::optional<lay_result> {
for (auto const share : {1.0, 0.75, 0.5, 0.35, 0.25})
if (auto laid{Layout(share)}; laid.has_value())
return laid;
return std::nullopt;
};
std::optional<lay_result> laid;
switch (Request.kind)
{
case lay_kind::straight: laid = first(straight); break;
case lay_kind::arc: laid = first(arc_through); break;
case lay_kind::straight_arc:
laid = first(straight_arc);
if (false == laid.has_value())
laid = first(arc_through);
break;
default:
laid = first(arc_straight);
if (false == laid.has_value())
laid = first(arc_through);
break;
}
if (laid.has_value())
return laid;
lay_result refused;
refused.note = std::abs(c0) >= 5.0 ? format(STR_C("No room to ramp the cant of %.0f mm the track ends with, the track is laid without transition curves"), std::abs(c0))
: std::string{STR_C("No room for the transition curves of the norms, the track is laid without them")};
return refused;
}
void recommend_between(vertex &Vertex, double const Speed, limits const &Limits)
{
if (false == Vertex.compound || Vertex.arcs.empty())
return;
auto sharpest{std::max(1.0, Vertex.radius)};
for (auto const &arc : Vertex.arcs)
sharpest = std::min(sharpest, std::max(1.0, arc.radius));
auto const speed{std::max(1.0, std::min(Speed, speed_limit(sharpest, Limits)))};
auto const cants{compound_cants(Vertex)};
auto previous{std::max(1.0, Vertex.radius)};
for (std::size_t i = 0; i < Vertex.arcs.size(); ++i)
{
auto &arc{Vertex.arcs[i]};
auto const radius{std::max(1.0, arc.radius)};
arc.transition = transition_between(speed, 1.0 / previous, cants[i], 1.0 / radius, cants[i + 1], Limits);
previous = radius;
}
}
void norm_transitions(vertex &Vertex, double const Speed, limits const &Limits)
{
if (Vertex.spirals)
return;
if (false == Vertex.compound || Vertex.arcs.empty())
{
Vertex.transition_in = Vertex.transition_out = transition_length(std::max(1.0, Speed), std::max(1.0, Vertex.radius), std::max(0.0, Vertex.cant), Limits);
return;
}
// at the speed the sharpest arc allows, as the warnings of compute check them
auto sharpest{std::max(1.0, Vertex.radius)};
for (auto const &arc : Vertex.arcs)
sharpest = std::min(sharpest, std::max(1.0, arc.radius));
auto const allowed{std::max(1.0, std::min(Speed, speed_limit(sharpest, Limits)))};
auto const cants{compound_cants(Vertex)};
Vertex.transition_in = transition_length(allowed, std::max(1.0, Vertex.radius), cants.front(), Limits);
Vertex.transition_out = transition_length(allowed, std::max(1.0, Vertex.arcs.back().radius), cants.back(), Limits);
recommend_between(Vertex, Speed, Limits);
}
bool tangent_intersection(design const &Design, glm::dvec2 &Point)
{
auto const start{plan_of(Design.start)};
auto const offset{plan_of(Design.end) - start};
auto const startdirection{glm::normalize(Design.start_direction)};
auto const enddirection{glm::normalize(Design.end_direction)};
auto const determinant{cross(startdirection, enddirection)};
if (std::abs(determinant) < 1e-9)
return false;
auto const t{cross(offset, enddirection) / determinant};
auto const u{cross(startdirection, offset) / determinant};
if (t <= 0.0 || u <= 0.0)
return false;
Point = start + startdirection * t;
return true;
}
bool spiral_tangents(transition_shape const Shape, double const TurnIn, double const TurnOut, double &TangentIn, double &TangentOut)
{
auto const length = [&](double const Turn) { return Shape == transition_shape::clothoid ? 2.0 * Turn : 2.0 * std::tan(Turn); };
if (TurnIn <= 0.0 || TurnOut <= 0.0 || TurnIn >= 0.5 * kPi || TurnOut >= 0.5 * kPi)
return false;
auto const in{transition_point(Shape, length(TurnIn), 1.0, 1.0)};
auto const out{transition_point(Shape, length(TurnOut), 1.0, 1.0)};
auto const deflection{in.angle + out.angle};
if (deflection >= kPi - 1e-3)
return false;
glm::dvec2 const direction{std::cos(deflection), std::sin(deflection)};
glm::dvec2 const normal{-direction.y, direction.x};
auto const end{glm::dvec2{in.x, in.y} + direction * out.x - normal * out.y};
TangentOut = end.y / std::sin(deflection);
TangentIn = end.x - TangentOut * std::cos(deflection);
return TangentIn > 0.0 && TangentOut > 0.0;
}
bool collinear(design const &Design)
{
auto const offset{plan_of(Design.end) - plan_of(Design.start)};
auto const startdirection{glm::normalize(Design.start_direction)};
auto const enddirection{glm::normalize(Design.end_direction)};
return std::abs(cross(startdirection, offset)) <= 0.01 && glm::dot(startdirection, enddirection) >= std::cos(1e-4) && glm::dot(startdirection, offset) > 0.0;
}
double elevation(result const &Result, double const Chainage)
{
auto const &profile{Result.profile};
if (profile.size() < 2)
return 0.0;
auto const s{std::clamp(Chainage, 0.0, Result.length)};
std::size_t segment{0};
while (segment + 2 < profile.size() && s > profile[segment + 1].chainage)
++segment;
auto const &from{profile[segment]};
auto const &to{profile[segment + 1]};
double height{from.elevation + (to.elevation - from.elevation) * (s - from.chainage) / (to.chainage - from.chainage)};
for (std::size_t i = 1; i + 1 < profile.size(); ++i)
{
auto const &point{profile[i]};
auto const distance{std::abs(s - point.chainage)};
if (point.tangent <= 0.0 || distance >= point.tangent)
continue;
auto const gradebefore{(point.elevation - profile[i - 1].elevation) / (point.chainage - profile[i - 1].chainage)};
auto const gradeafter{(profile[i + 1].elevation - point.elevation) / (profile[i + 1].chainage - point.chainage)};
auto const offset{(point.tangent - distance) * (point.tangent - distance) / (2.0 * point.radius)};
height += gradeafter > gradebefore ? offset : -offset;
}
return height;
}
double grade(result const &Result, double const Chainage)
{
auto const &profile{Result.profile};
if (profile.size() < 2)
return 0.0;
auto const s{std::clamp(Chainage, 0.0, Result.length)};
std::size_t segment{0};
while (segment + 2 < profile.size() && s > profile[segment + 1].chainage)
++segment;
auto const &from{profile[segment]};
auto const &to{profile[segment + 1]};
double slope{(to.elevation - from.elevation) / (to.chainage - from.chainage)};
for (std::size_t i = 1; i + 1 < profile.size(); ++i)
{
auto const &point{profile[i]};
auto const distance{s - point.chainage};
if (point.tangent <= 0.0 || std::abs(distance) >= point.tangent)
continue;
auto const gradebefore{(point.elevation - profile[i - 1].elevation) / (point.chainage - profile[i - 1].chainage)};
auto const gradeafter{(profile[i + 1].elevation - point.elevation) / (profile[i + 1].chainage - point.chainage)};
auto const change{(point.tangent - std::abs(distance)) / point.radius * (distance < 0.0 ? 1.0 : -1.0)};
slope += gradeafter > gradebefore ? change : -change;
}
return slope;
}
station evaluate(result const &Result, double const Chainage)
{
station result;
if (Result.elements.empty())
return result;
auto const s{std::clamp(Chainage, 0.0, Result.length)};
auto const &e{element_at(Result, s)};
auto const distance{std::clamp(s - e.chainage, 0.0, e.length)};
glm::dvec2 position;
glm::dvec2 direction;
element_point(e, Result.shape, distance, position, direction);
result.position = {position.x, elevation(Result, s), position.y};
result.direction = glm::normalize(glm::dvec3{direction.x, grade(Result, s), direction.y});
// a cant ramp of its own may lie on the straight before the curve, its cant turned the way the curve turns
auto const cant{signed_cant(Result, e, distance, false)};
result.cant = std::abs(cant);
result.turn = e.turn != 0 ? e.turn : (cant > 0.0) - (cant < 0.0);
return result;
}
namespace
{
// the plan from From to To in chords short enough to keep to the curves, the directions of its ends carried on far, so that a point
// past them is measured across the track
struct plan_line
{
std::vector<glm::dvec2> points;
std::vector<double> chainages;
};
plan_line sample_plan(result const &Result, double const From, double const To, double const Step)
{
constexpr double kFar{5000.0};
plan_line line;
if (Result.elements.empty())
return line;
auto const from{std::clamp(From, 0.0, Result.length)};
auto const to{std::clamp(To, from, Result.length)};
auto const push = [&](glm::dvec2 const &Position, double const Chainage) {
line.points.push_back(Position);
line.chainages.push_back(Chainage);
};
for (auto const &e : Result.elements)
{
auto const low{std::max(from, e.chainage)};
auto const high{std::min(to, e.chainage + e.length)};
if (high < low)
continue;
auto const count{std::max(1, static_cast<int>(std::ceil((high - low) / Step)))};
auto const at = [&](int const Index) { return low + (high - low) * Index / count; };
if (e.kind != element_kind::spiral)
{
glm::dvec2 position;
glm::dvec2 direction;
for (int i = 0; i <= count; ++i)
{
element_point(e, Result.shape, at(i) - e.chainage, position, direction);
push(position, at(i));
}
continue;
}
// spiral_point integrates from the start of the element for every point, here it goes on from the previous one
auto const heading = [&](double const S) { return e.curvature_start * S + (e.curvature_end - e.curvature_start) * S * S / (2.0 * std::max(e.length, 1e-9)); };
double x{0.0};
double y{0.0};
double s{0.0};
auto const advance = [&](double const Distance) {
auto const pieces{std::max(1, static_cast<int>(std::ceil((Distance - s) / 2.0)))};
auto const h{(Distance - s) / pieces};
for (int i = 0; i < pieces; ++i)
{
auto const a{heading(s + h * i)};
auto const m{heading(s + h * (i + 0.5))};
auto const b{heading(s + h * (i + 1))};
x += h / 6.0 * (std::cos(a) + 4.0 * std::cos(m) + std::cos(b));
y += h / 6.0 * (std::sin(a) + 4.0 * std::sin(m) + std::sin(b));
}
s = Distance;
};
for (int i = 0; i <= count; ++i)
{
advance(at(i) - e.chainage);
push(e.origin + e.direction * x + e.normal * y, at(i));
}
}
if (line.points.size() < 2)
return line;
auto const away = [](glm::dvec2 const &End, glm::dvec2 const &Inner) {
auto const offset{End - Inner};
return glm::length(offset) > 1e-9 ? offset / glm::length(offset) : glm::dvec2{0.0};
};
std::size_t first{1};
while (first + 1 < line.points.size() && glm::distance(line.points[first], line.points.front()) < 1e-6)
++first;
auto last{line.points.size() - 2};
while (last > 0 && glm::distance(line.points[last], line.points.back()) < 1e-6)
--last;
auto const before{away(line.points.front(), line.points[first])};
auto const after{away(line.points.back(), line.points[last])};
line.points.insert(line.points.begin(), line.points.front() + before * kFar);
line.chainages.insert(line.chainages.begin(), line.chainages.front() - kFar);
push(line.points.back() + after * kFar, line.chainages.back() + kFar);
return line;
}
plan_projection project_on(plan_line const &Line, glm::dvec2 const &Point)
{
plan_projection best;
auto nearest{std::numeric_limits<double>::max()};
for (std::size_t i = 0; i + 1 < Line.points.size(); ++i)
{
auto const segment{Line.points[i + 1] - Line.points[i]};
auto const length2{glm::dot(segment, segment)};
if (length2 < 1e-18)
continue;
auto const t{std::clamp(glm::dot(Point - Line.points[i], segment) / length2, 0.0, 1.0)};
auto const offset{Point - (Line.points[i] + segment * t)};
auto const distance2{glm::dot(offset, offset)};
if (distance2 >= nearest)
continue;
nearest = distance2;
best.chainage = Line.chainages[i] + (Line.chainages[i + 1] - Line.chainages[i]) * t;
best.offset = std::copysign(std::sqrt(distance2), cross(segment, offset));
}
return best;
}
double squares(std::vector<double> const &Values)
{
double sum{0.0};
for (auto const value : Values)
sum += value * value;
return sum;
}
using residuals_of = std::function<bool(std::vector<double> const &, std::vector<double> &)>;
// Levenberg-Marquardt: the parameters of the least sum of the squares of the residuals; a step to where Residuals fails isn't taken
double least_squares(std::vector<double> &Parameters, residuals_of const &Residuals)
{
constexpr double kDifference{1e-4};
std::vector<double> residuals;
if (false == Residuals(Parameters, residuals))
return std::numeric_limits<double>::max();
auto cost{squares(residuals)};
auto const n{Parameters.size()};
auto const m{residuals.size()};
double damping{1e-3};
std::vector<double> trial;
for (int iteration = 0; iteration < 40 && cost > 1e-12; ++iteration)
{
std::vector<std::vector<double>> jacobian(n, std::vector<double>(m, 0.0));
for (std::size_t j = 0; j < n; ++j)
{
auto parameters{Parameters};
parameters[j] += kDifference;
auto sign{1.0};
if (false == Residuals(parameters, trial) || trial.size() != m)
{
parameters[j] = Parameters[j] - kDifference;
sign = -1.0;
if (false == Residuals(parameters, trial) || trial.size() != m)
continue;
}
for (std::size_t i = 0; i < m; ++i)
jacobian[j][i] = sign * (trial[i] - residuals[i]) / kDifference;
}
std::vector<double> normal(n * n, 0.0);
std::vector<double> gradient(n, 0.0);
for (std::size_t j = 0; j < n; ++j)
{
for (std::size_t i = 0; i < m; ++i)
gradient[j] += jacobian[j][i] * residuals[i];
for (std::size_t k = 0; k < n; ++k)
for (std::size_t i = 0; i < m; ++i)
normal[j * n + k] += jacobian[j][i] * jacobian[k][i];
}
auto better{false};
auto settled{false};
for (int attempt = 0; attempt < 12 && false == better; ++attempt)
{
auto system{normal};
std::vector<double> step(n);
for (std::size_t j = 0; j < n; ++j)
{
system[j * n + j] += damping * std::max(normal[j * n + j], 1e-9);
step[j] = -gradient[j];
}
for (std::size_t c = 0; c < n; ++c)
{
auto pivot{c};
for (std::size_t r = c + 1; r < n; ++r)
if (std::abs(system[r * n + c]) > std::abs(system[pivot * n + c]))
pivot = r;
if (std::abs(system[pivot * n + c]) < 1e-300)
break;
for (std::size_t k = 0; k < n; ++k)
std::swap(system[c * n + k], system[pivot * n + k]);
std::swap(step[c], step[pivot]);
for (std::size_t r = c + 1; r < n; ++r)
{
auto const factor{system[r * n + c] / system[c * n + c]};
for (std::size_t k = c; k < n; ++k)
system[r * n + k] -= factor * system[c * n + k];
step[r] -= factor * step[c];
}
}
for (std::size_t c = n; c-- > 0;)
{
for (std::size_t k = c + 1; k < n; ++k)
step[c] -= system[c * n + k] * step[k];
step[c] = std::abs(system[c * n + c]) > 1e-300 ? step[c] / system[c * n + c] : 0.0;
}
auto candidate{Parameters};
for (std::size_t j = 0; j < n; ++j)
candidate[j] += step[j];
if (Residuals(candidate, trial) && trial.size() == m && squares(trial) < cost)
{
auto const lower{squares(trial)};
settled = cost - lower < 1e-6 * cost;
Parameters = candidate;
residuals = trial;
cost = lower;
damping = std::max(1e-9, damping * 0.3);
better = true;
}
else
{
damping *= 8.0;
}
}
if (false == better || settled)
break;
}
return cost;
}
bool compound_residuals(design const &Trial, std::vector<glm::dvec2> const &Points, std::vector<double> &Residuals);
constexpr double kTraceRadiusMin{15.0};
constexpr double kTraceRadiusMax{50000.0};
constexpr double kTraceRobust{0.25};
// m, how far the points clicked along a curve should be from the curve laid through them: a shape keeping to them no closer is
// given more radii
constexpr double kPointsWithin{0.3};
// m, how far they may be from it, clicked by hand far out: a row of bends making a curve no closer is cut in parts
constexpr double kPointsLoose{1.0};
// the parameters: the logarithms of the radii, then the weights of the shares of all but the first radius, as logarithms too.
// Between: the transitions between the arcs those of the norms, else the ones of the base
vertex traced_shape(design const &Design, vertex const &Base, std::vector<double> const &Parameters, int const Radii, bool const Between = true)
{
auto shape{Base};
auto const radius = [&](int const Index) { return std::exp(std::clamp(Parameters[Index], std::log(kTraceRadiusMin), std::log(kTraceRadiusMax))); };
shape.radius = radius(0);
shape.share = 1.0;
if (Radii == 1)
{
shape.compound = false;
return shape;
}
std::vector<double> weights{1.0};
for (int i = 1; i < Radii; ++i)
weights.push_back(std::exp(std::clamp(Parameters[Radii + i - 1], -5.0, 5.0)));
double total{0.0};
for (auto const weight : weights)
total += weight;
shape.compound = true;
shape.share = weights[0] / total;
shape.arcs.clear();
for (int i = 1; i < Radii; ++i)
{
compound_arc arc;
arc.radius = radius(i);
arc.transition = i - 1 < static_cast<int>(Base.arcs.size()) ? Base.arcs[i - 1].transition : 0.0;
arc.cant = i - 1 < static_cast<int>(Base.arcs.size()) ? Base.arcs[i - 1].cant : -1.0;
arc.share = weights[i] / total;
shape.arcs.push_back(arc);
}
if (Between && Design.norms.checked)
recommend_between(shape, Design.speed, Design.norms);
return shape;
}
// the departures of the points from the curve of the vertex shaped by the parameters, as traced_shape takes them
residuals_of trace_residuals(design const &Design, std::size_t const Vertex, std::vector<glm::dvec2> const &Points, int const Count, bool const Between)
{
return [&Design, &Points, Vertex, Count, Between](std::vector<double> const &Parameters, std::vector<double> &Residuals) {
auto trial{Design};
trial.norms.checked = false;
trial.vertices[Vertex] = traced_shape(Design, Design.vertices[Vertex], Parameters, Count, Between);
// a compound curve alone made straight away
if (Count >= 2 && trial.vertices.size() == 1)
return compound_residuals(trial, Points, Residuals);
auto const result{compute(trial)};
if (false == result.valid)
return false;
auto from{std::numeric_limits<double>::max()};
auto to{std::numeric_limits<double>::lowest()};
for (auto const &e : result.elements)
{
if (e.vertex != static_cast<int>(Vertex))
continue;
from = std::min(from, e.chainage);
to = std::max(to, e.chainage + e.length);
}
if (from > to)
return false;
auto const line{sample_plan(result, from - 1.0, to + 1.0, std::clamp((to - from) / 300.0, 0.25, 2.0))};
Residuals.clear();
for (auto const &point : Points)
{
// a point clicked by mistake far off pulls the curve less than the square of its departure would
auto const offset{project_on(line, point).offset};
auto const departure{std::abs(offset)};
Residuals.push_back(departure <= kTraceRobust ? offset : std::copysign(std::sqrt(2.0 * kTraceRobust * departure - kTraceRobust * kTraceRobust), offset));
}
return true;
};
}
// the curve of the vertex given the shape, and how far the points are from it
trace_fit trace_of(design const &Design, std::size_t const Vertex, vertex const &Shape, std::vector<glm::dvec2> const &Points)
{
trace_fit fit;
auto trial{Design};
trial.vertices[Vertex] = Shape;
auto const result{compute(trial)};
if (false == result.valid)
return fit;
fit.valid = true;
fit.fitted = Shape;
double sum{0.0};
for (auto const &projection : project(result, Points))
{
sum += projection.offset * projection.offset;
fit.worst = std::max(fit.worst, std::abs(projection.offset));
}
fit.rms = std::sqrt(sum / static_cast<double>(Points.size()));
return fit;
}
} // namespace
std::vector<plan_projection> project(result const &Result, std::vector<glm::dvec2> const &Points)
{
std::vector<plan_projection> projections;
if (Result.elements.empty())
return projections;
auto const line{sample_plan(Result, 0.0, Result.length, std::max(0.5, Result.length / 4000.0))};
for (auto const &point : Points)
projections.push_back(project_on(line, point));
return projections;
}
trace_fit fit_trace(design const &Design, std::size_t const Vertex, std::vector<glm::dvec2> const &Points, int const Radii)
{
trace_fit best;
if (Vertex >= Design.vertices.size() || Points.empty())
return best;
auto const &base{Design.vertices[Vertex]};
auto const residuals = [&](int const Count) { return trace_residuals(Design, Vertex, Points, Count, true); };
auto const cost_of = [](residuals_of const &Residuals, std::vector<double> const &Parameters) {
std::vector<double> values;
return Residuals(Parameters, values) ? squares(values) : std::numeric_limits<double>::max();
};
// one radius: a scan over the whole range, then the golden section around the best of it
auto const one{residuals(1)};
auto const low{std::log(kTraceRadiusMin)};
auto const high{std::log(kTraceRadiusMax)};
constexpr int kScan{60};
auto const spacing{(high - low) / kScan};
auto bestlog{std::log(std::max(kTraceRadiusMin, base.radius))};
auto bestcost{cost_of(one, {bestlog})};
for (int i = 0; i <= kScan; ++i)
if (auto const cost{cost_of(one, {low + spacing * i})}; cost < bestcost)
{
bestcost = cost;
bestlog = low + spacing * i;
}
if (bestcost == std::numeric_limits<double>::max())
return best;
{
constexpr double kGolden{0.6180339887498949};
auto a{bestlog - spacing};
auto b{bestlog + spacing};
auto c{b - (b - a) * kGolden};
auto d{a + (b - a) * kGolden};
auto fc{cost_of(one, {c})};
auto fd{cost_of(one, {d})};
for (int i = 0; i < 50; ++i)
{
if (fc < fd)
{
b = d;
d = c;
fd = fc;
c = b - (b - a) * kGolden;
fc = cost_of(one, {c});
}
else
{
a = c;
c = d;
fc = fd;
d = a + (b - a) * kGolden;
fd = cost_of(one, {d});
}
}
if (auto const middle{(a + b) * 0.5}; cost_of(one, {middle}) < bestcost)
bestlog = middle;
}
struct candidate
{
std::vector<double> parameters;
double cost{std::numeric_limits<double>::max()};
};
std::array<candidate, 4> fits;
fits[1] = {{bestlog}, cost_of(one, {bestlog})};
auto const most{std::clamp(Radii > 0 ? Radii : 3, 1, std::max(1, static_cast<int>(Points.size()) / 2))};
auto const shift{std::log(1.3)};
auto const share = [](double const Second, double const First) { return std::log(Second / First); };
auto const rms = [&](int const Count) { return std::sqrt(fits[Count].cost / static_cast<double>(Points.size())); };
// the next radius has to halve the departure about, a curve which keeps to the points already gets none
constexpr double kClose{0.08};
auto chosen{1};
auto const wanted = [&](int const Count) { return Count <= most && (Radii > 0 || rms(chosen) > kClose); };
auto const take = [&](int const Count) {
if (fits[Count].cost == std::numeric_limits<double>::max())
return;
if (Radii > 0 || rms(Count) < 0.6 * rms(chosen))
chosen = Count;
};
if (wanted(2))
{
auto const two{residuals(2)};
for (auto const fraction : {0.3, 0.5, 0.7})
for (auto const sign : {-1.0, 1.0})
{
std::vector<double> parameters{bestlog + sign * shift, bestlog - sign * shift, share(1.0 - fraction, fraction)};
if (auto const cost{least_squares(parameters, two)}; cost < fits[2].cost)
fits[2] = {parameters, cost};
}
take(2);
}
if (wanted(3))
{
auto const three{residuals(3)};
std::vector<std::vector<double>> starts;
for (auto const sign : {-1.0, 1.0})
starts.push_back({bestlog + sign * shift, bestlog - sign * shift, bestlog + sign * shift, 0.0, 0.0});
if (false == fits[2].parameters.empty())
{
auto const &two{fits[2].parameters};
auto const second{std::exp(two[2])};
starts.push_back({two[0], two[0], two[1], 0.0, std::log(2.0 * second)});
starts.push_back({two[0], two[1], two[1], std::log(0.5 * second), std::log(0.5 * second)});
}
for (auto parameters : starts)
if (auto const cost{least_squares(parameters, three)}; cost < fits[3].cost)
fits[3] = {parameters, cost};
take(3);
}
auto shape{traced_shape(Design, base, fits[chosen].parameters, chosen)};
shape.radius = std::max(kTraceRadiusMin, std::round(shape.radius));
for (auto &arc : shape.arcs)
arc.radius = std::max(kTraceRadiusMin, std::round(arc.radius));
if (Design.norms.checked)
recommend_between(shape, Design.speed, Design.norms);
return trace_of(Design, Vertex, shape, Points);
}
trace_fit refit_radii(design const &Design, std::size_t const Vertex, std::vector<glm::dvec2> const &Points)
{
if (Vertex >= Design.vertices.size() || Points.empty())
return {};
auto const &base{Design.vertices[Vertex]};
auto const compound{base.compound && false == base.arcs.empty()};
auto const count{compound ? static_cast<int>(base.arcs.size()) + 1 : 1};
// started from the curve as it is: the radii, then the shares weighed against the one of the first arc
std::vector<double> parameters{std::log(std::max(kTraceRadiusMin, base.radius))};
if (compound)
{
for (auto const &arc : base.arcs)
parameters.push_back(std::log(std::max(kTraceRadiusMin, arc.radius)));
for (auto const &arc : base.arcs)
parameters.push_back(std::log(std::max(1e-3, arc.share) / std::max(1e-3, base.share)));
}
least_squares(parameters, trace_residuals(Design, Vertex, Points, count, false));
auto shape{traced_shape(Design, base, parameters, count, false)};
shape.radius = std::max(kTraceRadiusMin, std::round(shape.radius));
for (auto &arc : shape.arcs)
arc.radius = std::max(kTraceRadiusMin, std::round(arc.radius));
return trace_of(Design, Vertex, shape, Points);
}
std::vector<double> breaks_at(result const &Result, std::vector<glm::dvec2> const &Points, double const From, double const To)
{
double constexpr apart{0.2};
std::vector<double> breaks;
auto previous{From};
for (auto const &at : project(Result, Points))
{
if (at.chainage < previous + apart || at.chainage > To - apart)
return {};
breaks.push_back(previous = at.chainage);
}
return breaks;
}
std::vector<segment_data> pieces_at(result const &Result, design const &Design, std::size_t const Count, std::vector<glm::dvec2> const &Joints, std::vector<std::size_t> &Intervals)
{
Intervals.clear();
auto const breaks{breaks_at(Result, Joints, 0.0, Result.length)};
if (breaks.empty() || breaks.size() != Joints.size())
return pieces(Result, Design, Count);
return pieces(Result, Design, Count, breaks, std::vector<std::size_t>(breaks.size() + 1, 1), &Intervals);
}
std::size_t minimum_pieces(result const &Result, design const &Design)
{
std::size_t count{0};
for (auto const &e : Result.elements)
count += element_pieces(e, Design.shape, Design.transition_pieces, Design.arc_piece_angle);
return count;
}
std::vector<segment_data> pieces(result const &Result, design const &Design, std::size_t const Count, std::vector<double> const &Breaks, std::vector<std::size_t> const &Counts,
std::vector<std::size_t> *Intervals)
{
struct span
{
std::size_t element;
double from;
double to;
bool generic{false};
double start{0.0};
double end{0.0};
};
auto const extent = [&](span const &Span) { return Span.generic ? Span.end - Span.start : Span.to - Span.from; };
auto const chainage_from = [&](span const &Span) { return Span.generic ? Span.start : Result.elements[Span.element].chainage + Span.from; };
auto const chainage_to = [&](span const &Span) { return Span.generic ? Span.end : Result.elements[Span.element].chainage + Span.to; };
std::vector<span> spans;
std::optional<double> lead;
for (std::size_t i = 0; i < Result.elements.size(); ++i)
{
auto const &e{Result.elements[i]};
auto const count{element_pieces(e, Design.shape, Design.transition_pieces, Design.arc_piece_angle)};
if (count == 0)
{
if (e.length <= 0.0)
continue;
if (spans.empty())
{
lead = lead.value_or(e.chainage);
continue;
}
auto &previous{spans.back()};
previous.start = chainage_from(previous);
previous.end = e.chainage + e.length;
previous.generic = true;
continue;
}
for (std::size_t j = 0; j < count; ++j)
spans.push_back({i, e.length * j / count, e.length * (j + 1) / count});
if (lead.has_value())
{
auto &first{spans[spans.size() - count]};
first.end = chainage_to(first);
first.start = *lead;
first.generic = true;
lead.reset();
}
}
double const shortest{1.0};
for (std::size_t i = 0; i < spans.size() && spans.size() > 1;)
{
auto const &e{Result.elements[spans[i].element]};
if (e.kind != element_kind::straight || extent(spans[i]) >= shortest)
{
++i;
continue;
}
auto const start{chainage_from(spans[i])};
auto const end{chainage_to(spans[i])};
if (i > 0)
{
auto &previous{spans[i - 1]};
previous.start = chainage_from(previous);
previous.end = end;
previous.generic = true;
}
else
{
auto &next{spans[i + 1]};
next.end = chainage_to(next);
next.start = start;
next.generic = true;
}
spans.erase(spans.begin() + i);
}
// splits the span at the chainage
auto const split = [&](std::vector<span>::iterator Span, double const Chainage) {
span second{*Span};
if (Span->generic)
{
second.start = Chainage;
Span->end = Chainage;
}
else
{
auto const local{Chainage - Result.elements[Span->element].chainage};
second.from = local;
Span->to = local;
}
spans.insert(std::next(Span), second);
};
double constexpr margin{0.01};
// the ends of the cant ramps of their own, where the cant starts or stops changing along a straight, a transition or an arc
for (auto const &ramp : Result.ramps)
for (auto const chainage : {ramp.from, ramp.to})
if (auto inside{std::find_if(spans.begin(), spans.end(), [&](span const &Span) { return chainage_from(Span) < chainage - margin && chainage_to(Span) > chainage + margin; })};
ramp.own && inside != spans.end())
split(inside, chainage);
for (auto const chainage : Breaks)
{
auto inside{std::find_if(spans.begin(), spans.end(), [&](span const &Span) { return chainage_from(Span) < chainage - margin && chainage_to(Span) > chainage + margin; })};
if (inside != spans.end())
split(inside, chainage);
}
auto const interval = [&](span const &Span) {
return static_cast<std::size_t>(std::upper_bound(Breaks.begin(), Breaks.end(), (chainage_from(Span) + chainage_to(Span)) * 0.5) - Breaks.begin());
};
for (std::size_t k = 0; k < Counts.size(); ++k)
{
while (true)
{
auto const inside{static_cast<std::size_t>(std::count_if(spans.begin(), spans.end(), [&](span const &Span) { return interval(Span) == k; }))};
if (inside == 0 || inside >= Counts[k])
break;
auto longest{spans.end()};
for (auto candidate = spans.begin(); candidate != spans.end(); ++candidate)
if (interval(*candidate) == k && (longest == spans.end() || extent(*candidate) > extent(*longest)))
longest = candidate;
split(longest, (chainage_from(*longest) + chainage_to(*longest)) * 0.5);
}
}
while (false == spans.empty() && spans.size() < Count)
{
auto longest = std::max_element(spans.begin(), spans.end(), [&](span const &A, span const &B) { return extent(A) < extent(B); });
split(longest, (chainage_from(*longest) + chainage_to(*longest)) * 0.5);
}
// no path longer than a track may be
for (std::size_t i = 0; i < spans.size();)
{
auto const from{chainage_from(spans[i])};
auto const length{chainage_to(spans[i]) - from};
if (length <= geometry::longest_path)
{
++i;
continue;
}
auto const count{static_cast<int>(std::ceil(length / geometry::longest_path))};
split(spans.begin() + static_cast<std::ptrdiff_t>(i), from + length / count);
}
if (Intervals != nullptr)
{
Intervals->clear();
for (auto const &piece : spans)
Intervals->push_back(interval(piece));
}
std::vector<segment_data> result;
for (auto const &piece : spans)
{
if (piece.generic)
{
auto const first{evaluate(Result, piece.start)};
auto const last{evaluate(Result, piece.end)};
glm::dvec2 const p0{first.position.x, first.position.z};
glm::dvec2 const p3{last.position.x, last.position.z};
auto const d0{glm::normalize(glm::dvec2{first.direction.x, first.direction.z})};
auto const d3{glm::normalize(glm::dvec2{last.direction.x, last.direction.z})};
auto const handle{glm::distance(p0, p3) / 3.0};
auto const p1{p0 + d0 * handle};
auto const p2{p3 - d3 * handle};
auto const y0{first.position.y};
auto const y3{last.position.y};
auto const y1{y0 + grade(Result, piece.start) * handle};
auto const y2{y3 - grade(Result, piece.end) * handle};
auto const roll = [&](station const &Station) { return static_cast<float>(Station.turn * glm::degrees(std::asin(std::clamp(Station.cant / Design.norms.gauge, -1.0, 1.0)))); };
segment_data path;
path.points[segment_data::point::start] = first.position;
path.points[segment_data::point::end] = last.position;
path.points[segment_data::point::control1] = glm::dvec3{p1.x, y1, p1.y} - first.position;
path.points[segment_data::point::control2] = glm::dvec3{p2.x, y2, p2.y} - last.position;
path.rolls[0] = roll(first);
path.rolls[1] = roll(last);
auto const &e{Result.elements[piece.element]};
path.radius = e.kind == element_kind::straight ? 0.f : static_cast<float>(e.radius);
result.push_back(path);
continue;
}
auto const &e{Result.elements[piece.element]};
glm::dvec2 p0;
glm::dvec2 p3;
glm::dvec2 d0;
glm::dvec2 d3;
element_point(e, Design.shape, piece.from, p0, d0);
element_point(e, Design.shape, piece.to, p3, d3);
glm::dvec2 p1;
glm::dvec2 p2;
switch (e.kind)
{
case element_kind::straight:
p1 = p0 + (p3 - p0) / 3.0;
p2 = p0 + (p3 - p0) * (2.0 / 3.0);
break;
case element_kind::arc:
{
auto const angle{(piece.to - piece.from) / e.radius};
auto const handle{4.0 / 3.0 * std::tan(angle / 4.0) * e.radius};
p1 = p0 + d0 * handle;
p2 = p3 - d3 * handle;
break;
}
default:
{
if (Design.shape == transition_shape::cubic_parabola && e.kind != element_kind::spiral)
{
std::array<glm::dvec2, 4> const local{glm::dvec2{0.0, 0.0}, glm::dvec2{e.transition / 3.0, 0.0}, glm::dvec2{e.transition * 2.0 / 3.0, 0.0}, glm::dvec2{e.transition, e.transition * e.transition / (6.0 * e.radius)}};
auto const a{e.kind == element_kind::transition_in ? piece.from / e.transition : 1.0 - piece.from / e.transition};
auto const b{e.kind == element_kind::transition_in ? piece.to / e.transition : 1.0 - piece.to / e.transition};
auto const world = [&](glm::dvec2 const &Point) { return e.origin + e.direction * Point.x + e.normal * Point.y; };
p0 = world(blossom(local, a, a, a));
p1 = world(blossom(local, a, a, b));
p2 = world(blossom(local, a, b, b));
p3 = world(blossom(local, b, b, b));
}
else
{
auto const chord{glm::distance(p0, p3) / 3.0};
p1 = p0 + d0 * chord;
p2 = p3 - d3 * chord;
}
break;
}
}
auto const s0{e.chainage + piece.from};
auto const s3{e.chainage + piece.to};
auto const y0{elevation(Result, s0)};
auto const y3{elevation(Result, s3)};
auto const y1{y0 + grade(Result, s0) * glm::distance(p0, p1)};
auto const y2{y3 - grade(Result, s3) * glm::distance(p2, p3)};
// the cant signed with the turn, a ramp of its own on a straight too; where it changes all at once, the one of the piece itself
auto const roll = [&](double const Cant) { return static_cast<float>(glm::degrees(std::asin(std::clamp(Cant / Design.norms.gauge, -1.0, 1.0)))); };
segment_data path;
path.points[segment_data::point::start] = {p0.x, y0, p0.y};
path.points[segment_data::point::end] = {p3.x, y3, p3.y};
path.points[segment_data::point::control1] = glm::dvec3{p1.x, y1, p1.y} - path.points[segment_data::point::start];
path.points[segment_data::point::control2] = glm::dvec3{p2.x, y2, p2.y} - path.points[segment_data::point::end];
path.rolls[0] = roll(signed_cant(Result, e, piece.from, false));
path.rolls[1] = roll(signed_cant(Result, e, piece.to, true));
path.radius = e.kind == element_kind::straight ? 0.f : static_cast<float>(e.radius);
if (e.kind == element_kind::spiral)
{
auto const curvature{e.curvature_start + (e.curvature_end - e.curvature_start) * (piece.from + piece.to) * 0.5 / std::max(e.length, 1e-9)};
path.radius = curvature > 1e-9 ? static_cast<float>(1.0 / curvature) : 0.f;
}
result.push_back(path);
}
return result;
}
namespace
{
// the vertices where fit_between puts them, the start before them and the end after; empty where it can't
std::vector<glm::dvec2> polygon_of(design const &Design)
{
if (glm::length(Design.start_direction) < 1e-9 || glm::length(Design.end_direction) < 1e-9)
return {};
auto const count{Design.vertices.size()};
auto const start{plan_of(Design.start)};
auto const end{plan_of(Design.end)};
std::vector<glm::dvec2> polygon{start};
if (count == 1)
{
glm::dvec2 corner;
if (false == tangent_intersection(Design, corner))
return {};
polygon.push_back(corner);
}
else if (count >= 2)
{
polygon.push_back(start + glm::normalize(Design.start_direction) * Design.vertices.front().offset);
for (std::size_t i = 1; i + 1 < count; ++i)
polygon.push_back(Design.vertices[i].position);
polygon.push_back(end - glm::normalize(Design.end_direction) * Design.vertices.back().offset);
}
polygon.push_back(end);
return polygon;
}
// the angle the line turns by at the vertex, nought where it can't be told
double vertex_deflection(design const &Design, std::size_t const Vertex)
{
auto const polygon{polygon_of(Design)};
if (Vertex + 2 >= polygon.size())
return 0.0;
auto const in{polygon[Vertex + 1] - polygon[Vertex]};
auto const out{polygon[Vertex + 2] - polygon[Vertex + 1]};
if (glm::length(in) < 1e-9 || glm::length(out) < 1e-9)
return 0.0;
return std::atan2(std::abs(cross(in, out)), glm::dot(in, out));
}
// the cant the transitions of the length can ramp at the speed, in steps of 5 mm
double ramped_cant(double const Cant, double const Length, double const Speed, limits const &Norms)
{
auto const cant{std::min({Cant, Length * 1000.0 / (Norms.ramp_factor * Speed), Norms.cant_rate * 3.6 * Length / Speed})};
return cant < 5.0 ? 0.0 : std::floor(cant / 5.0) * 5.0;
}
// the transitions and the cant of the norms for the radii of the vertex, at the speed its sharpest arc allows
void recommended_curve(vertex &Vertex, double const Speed, limits const &Norms)
{
auto sharpest{std::max(1.0, Vertex.radius)};
if (Vertex.compound)
for (auto const &arc : Vertex.arcs)
sharpest = std::min(sharpest, std::max(1.0, arc.radius));
auto const allowed{std::max(1.0, std::min(Speed, speed_limit(sharpest, Norms)))};
auto const recommended{recommend(allowed, sharpest, Norms)};
Vertex.transition_in = Vertex.transition_out = recommended.transition;
Vertex.cant = ramped_cant(recommended.cant, recommended.transition, allowed, Norms);
recommend_between(Vertex, Speed, Norms);
}
// a curve a bend can be eased with, and how far it reaches along the straights either side of it
struct corner_shape
{
bool spirals{false};
double radius{0.0};
double transition{0.0};
double cant{0.0};
double reach{0.0};
};
// the bends eased one by one, each with the curve as large as fits between its neighbours
result ease_bends(design &Design, std::vector<char> const &Corners, corner_radii const &Radii)
{
auto const count{Design.vertices.size()};
auto const corner = [&](std::size_t const K) { return K < Corners.size() && Corners[K] != 0; };
// the curves of the vertices left as they are reach along the straights as far as they do with the bends kept out of their way
std::vector<std::array<double, 2>> kept(count, {0.0, 0.0});
{
auto bare{Design};
for (std::size_t k = 0; k < count; ++k)
if (corner(k))
bare.vertices[k] = {bare.vertices[k].position, bare.vertices[k].offset, 1.0};
for (auto const &curve : compute(bare).curves)
if (curve.vertex >= 0 && curve.vertex < static_cast<int>(count))
kept[curve.vertex] = {curve.tangent_in, curve.tangent_out};
}
auto const polygon{polygon_of(Design)};
if (polygon.size() != count + 2)
return compute(Design);
auto const &norms{Design.norms};
auto const speed{std::max(1.0, Design.speed)};
auto const low{std::max(10.0, std::min(Radii.low, Radii.high))};
auto const high{std::max(low, Radii.high)};
// the shapes each bend can take, the best first
std::vector<std::vector<corner_shape>> ladders(count);
for (std::size_t k = 0; k < count; ++k)
{
if (false == corner(k))
continue;
auto const in{polygon[k + 1] - polygon[k]};
auto const out{polygon[k + 2] - polygon[k + 1]};
if (glm::length(in) < 1e-6 || glm::length(out) < 1e-6)
continue;
auto const directionin{glm::normalize(in)};
auto const directionout{glm::normalize(out)};
auto const turn{cross(directionin, directionout)};
auto const deflection{std::atan2(std::abs(turn), glm::dot(directionin, directionout))};
int const side{turn > 0.0 ? 1 : -1};
if (deflection < 1e-7 || deflection > kPi - 1e-3)
continue;
auto const reach = [&](double const Length, double const Radius) {
double tangentin;
double tangentout;
curve_tangents(Design.shape, Length, Length, Radius, directionin, directionout, side, tangentin, tangentout);
return std::max(tangentin, tangentout);
};
auto &ladder{ladders[k]};
// two transitions meeting with no cant, long enough for the acceleration across to change gently at the speed: a bend too
// slight for them at the highest radius takes a larger one
auto const long_enough = [&](double const Radius) { return spirals_length(Design.shape, deflection, Radius) >= transition_length(speed, Radius, 0.0, norms) - 1e-6; };
auto top{high};
if (false == long_enough(top))
{
auto below{top};
auto above{top * 2.0};
while (false == long_enough(above) && above < 1e7)
{
below = above;
above *= 2.0;
}
for (int i = 0; i < 40; ++i)
{
auto const middle{(below + above) * 0.5};
(long_enough(middle) ? above : below) = middle;
}
top = std::ceil(above / 10.0) * 10.0;
}
constexpr int kSpirals{30};
for (int i = 0; i <= kSpirals; ++i)
{
auto const radius{std::round(top * std::pow(low / top, static_cast<double>(i) / kSpirals))};
if (false == long_enough(radius))
break;
auto const length{spirals_length(Design.shape, deflection, radius)};
ladder.push_back({true, radius, length, 0.0, reach(length, radius)});
if (top <= low)
break;
}
// then arcs with the transitions and the cant of the norms, as large as fit, to any radius: the transitions shortened the way
// fit_between does it, to leave the arc its least length
auto const keep_arc{std::max(norms.arc_min, norms.arc_min_factor * speed)};
constexpr int kArcs{60};
for (int i = 0; i <= kArcs; ++i)
{
auto const radius{std::round(high * std::pow(10.0 / high, static_cast<double>(i) / kArcs))};
auto const allowed{std::max(1.0, std::min(speed, speed_limit(radius, norms)))};
auto const recommended{recommend(allowed, radius, norms)};
auto const turn_of = [&](double const Length) { return 2.0 * transition_point(Design.shape, Length, radius, 1.0).angle; };
auto const least{std::min(radius * deflection, keep_arc)};
auto length{recommended.transition};
if (length > 0.0 && radius * (deflection - turn_of(length)) < least - 1e-6)
{
double shorter{0.0};
double longer{length};
for (int j = 0; j < 50; ++j)
{
auto const middle{(shorter + longer) * 0.5};
(radius * (deflection - turn_of(middle)) < least ? longer : shorter) = middle;
}
length = shorter < 1.0 ? 0.0 : shorter;
}
auto const cant{length > 0.0 ? ramped_cant(recommended.cant, length, allowed, norms) : 0.0};
ladder.push_back({false, radius, length, cant, reach(length, radius)});
}
}
// the straight between two curves is shared: each of them gets what it wants of it where they both fit, else the one wanting less
// than a half gets it, the other the rest, or each a half. Then each bend takes the best shape the room its neighbours leave fits
auto const between{norms.tangent_min_time * speed / 3.6};
auto const leg = [&](std::size_t const Leg) {
return glm::distance(polygon[Leg], polygon[Leg + 1]) + (Leg == 0 ? std::max(0.0, Design.start_reserve) : 0.0) + (Leg == count ? std::max(0.0, Design.end_reserve) : 0.0) -
(Leg > 0 && Leg < count ? between : 0.0);
};
std::vector<std::size_t> picks(count, 0);
auto const reach_of = [&](std::size_t const K, bool const Out) {
if (false == ladders[K].empty())
return ladders[K][picks[K]].reach;
return corner(K) ? 0.0 : kept[K][Out ? 1 : 0];
};
auto const best_within = [&](std::size_t const K, double const Room) {
auto const &ladder{ladders[K]};
for (std::size_t i = 0; i < ladder.size(); ++i)
if (ladder[i].reach <= Room + 1e-6)
return i;
return ladder.size() - 1;
};
std::vector<double> roomin(count, std::numeric_limits<double>::max());
std::vector<double> roomout(count, std::numeric_limits<double>::max());
for (std::size_t j = 0; j <= count; ++j)
{
auto const room{std::max(0.0, leg(j))};
auto const before{j > 0 ? reach_of(j - 1, true) : 0.0};
auto const after{j < count ? reach_of(j, false) : 0.0};
auto give_before{room - after};
auto give_after{room - before};
if (before + after > room)
{
auto const half{room * 0.5};
give_before = after <= half ? room - after : before <= half ? before : half;
give_after = before <= half ? room - before : after <= half ? after : half;
}
if (j > 0)
roomout[j - 1] = give_before;
if (j < count)
roomin[j] = give_after;
}
for (std::size_t k = 0; k < count; ++k)
if (false == ladders[k].empty())
picks[k] = best_within(k, std::min(roomin[k], roomout[k]));
for (int pass = 0; pass < 3; ++pass)
for (std::size_t k = 0; k < count; ++k)
{
if (ladders[k].empty())
continue;
auto const in{leg(k) - (k > 0 ? reach_of(k - 1, true) : 0.0)};
auto const out{leg(k + 1) - (k + 1 < count ? reach_of(k + 1, false) : 0.0)};
picks[k] = best_within(k, std::min(in, out));
}
for (std::size_t k = 0; k < count; ++k)
{
if (ladders[k].empty())
continue;
auto const &shape{ladders[k][picks[k]]};
auto &vertex{Design.vertices[k]};
vertex.spirals = shape.spirals;
vertex.radius = shape.radius;
vertex.transition_in = vertex.transition_out = shape.transition;
vertex.cant = shape.cant;
vertex.reverse_turn = false;
vertex.compound = false;
vertex.share = 1.0;
vertex.arcs.clear();
}
return compute(Design);
}
// the curves the line lays at its vertices: the side each turns to, where each begins and ends
struct laid_curve
{
int vertex{-1};
int side{0};
double from{0.0};
double to{0.0};
};
std::vector<laid_curve> laid_curves(result const &Line)
{
std::vector<laid_curve> curves;
for (auto const &e : Line.elements)
{
if (e.vertex < 0 || e.kind == element_kind::straight)
continue;
if (curves.empty() || curves.back().vertex != e.vertex)
curves.push_back({e.vertex, e.turn, e.chainage, e.chainage + e.length});
else
curves.back().to = e.chainage + e.length;
}
return curves;
}
// the shortest straight the line keeps between two curves turning the same way, as many metres as km/h of the speed, 30 m at least:
// two curves with a shorter one between them are one compound curve
double shortest_between(design const &Design)
{
return std::max(30.0, Design.speed);
}
} // namespace
result ease_corners(design &Design, std::vector<char> &Corners, corner_radii const &Radii, stretch_fit const &Fit)
{
// a point following a slight bend clicked closer than the curves leave a straight of the shortest length was clicked on a curve
// seen on the photo: the bends of a row of such, turning the same way, are the points of a curve traced through them, between
// the straights before and after them, one compound curve rather than short straights between curves, or several where one
// doesn't keep to them
auto line{ease_bends(Design, Corners, Radii)};
auto const shortest{shortest_between(Design)};
auto const bend = [&](int const K) { return K >= 0 && K < static_cast<int>(Corners.size()) && Corners[K] == 1; };
auto pending{false};
for (int pass = 0; pass < 32 && line.valid && false == pending; ++pass)
{
auto const curves{laid_curves(line)};
auto const polygon{polygon_of(Design)};
if (polygon.size() != Design.vertices.size() + 2)
break;
// the rows of bends: each as a pair of the first and the last curve, the longest first
std::vector<std::pair<std::size_t, std::size_t>> rows;
for (std::size_t i = 0; i < curves.size();)
{
auto j{i};
while (j + 1 < curves.size() && bend(curves[i].vertex) && bend(curves[j + 1].vertex) && curves[j + 1].vertex == curves[j].vertex + 1 && curves[j + 1].side == curves[i].side &&
curves[j + 1].from - curves[j].to < shortest)
++j;
if (j > i)
rows.emplace_back(i, j);
i = j + 1;
}
auto merged{false};
for (auto row{rows.begin()}; row != rows.end() && false == merged && false == pending; ++row)
{
// a row which doesn't make one curve is tried in parts, cut at its longest straight
std::vector<std::pair<std::size_t, std::size_t>> parts{*row};
while (false == parts.empty() && false == merged && false == pending)
{
auto const [first, last]{parts.back()};
parts.pop_back();
if (first == last)
continue;
auto const k{curves[first].vertex};
auto const m{curves[last].vertex};
design base{Design};
base.vertices.clear();
base.start = {polygon[k].x, Design.start.y, polygon[k].y};
base.start_direction = glm::normalize(polygon[k + 1] - polygon[k]);
base.end = {polygon[m + 2].x, Design.end.y, polygon[m + 2].y};
base.end_direction = glm::normalize(polygon[m + 2] - polygon[m + 1]);
base.start_reserve = 0.0;
base.end_reserve = 0.0;
std::vector<glm::dvec2> points(polygon.begin() + k + 1, polygon.begin() + m + 2);
// two bends don't show the curve between them: the curve goes through the line their two curves laid, with no
// straight between them, from where the first begins to where the second ends, the straights either side its room
auto within{kPointsLoose};
if (last == first + 1)
{
auto const from{curves[first].from};
auto const to{curves[last].to};
points.clear();
auto const count{std::max(4, static_cast<int>(std::ceil((to - from) / 5.0)))};
for (int i = 0; i <= count; ++i)
{
auto const station{evaluate(line, from + (to - from) * i / count)};
points.push_back(plan_of(station.position));
}
auto const before{evaluate(line, from)};
auto const after{evaluate(line, to)};
base.start = before.position;
base.start_direction = glm::normalize(plan_of(before.direction));
base.end = after.position;
base.end_direction = glm::normalize(plan_of(after.direction));
base.start_reserve = from - (first > 0 ? curves[first - 1].to : 0.0);
base.end_reserve = (last + 1 < curves.size() ? curves[last + 1].from : line.length) - to;
within = kPointsWithin;
}
auto const fit{Fit ? Fit(base, points) : std::optional<trace_stretch>{fit_trace_stretch(base, points, 0, false)}};
// still being fitted on the thread: the bends stay apart until it's done
if (false == fit.has_value())
{
pending = true;
break;
}
if (fit->valid && fit->vertices.size() == 1 && fit->worst <= within)
{
// the row of bends one vertex, kept as it is
std::vector<vertex> vertices(Design.vertices.begin(), Design.vertices.begin() + k);
vertices.push_back(fit->vertices.front());
vertices.insert(vertices.end(), Design.vertices.begin() + m + 1, Design.vertices.end());
std::vector<char> corners(Corners.begin(), Corners.begin() + k);
corners.push_back(0);
corners.insert(corners.end(), Corners.begin() + m + 1, Corners.end());
for (std::size_t i = 0; i < vertices.size(); ++i)
if (i < static_cast<std::size_t>(k) || i > static_cast<std::size_t>(k))
vertices[i].position = polygon[(i < static_cast<std::size_t>(k) ? i : i + (m - k)) + 1];
if (vertices.size() >= 2)
{
vertices.front().offset = glm::dot(vertices.front().position - plan_of(Design.start), glm::normalize(Design.start_direction));
vertices.back().offset = glm::dot(plan_of(Design.end) - vertices.back().position, glm::normalize(Design.end_direction));
}
Design.vertices = std::move(vertices);
Corners = std::move(corners);
merged = true;
break;
}
if (last - first < 2)
continue;
auto cut{first};
for (auto i{first}; i < last; ++i)
if (curves[i + 1].from - curves[i].to > curves[cut + 1].from - curves[cut].to)
cut = i;
parts.emplace_back(first, cut);
parts.emplace_back(cut + 1, last);
}
}
if (false == merged || pending)
break;
line = ease_bends(Design, Corners, Radii);
}
return line;
}
namespace
{
// a curve a single radius follows the points about as closely as this needs no more of them
constexpr double kStretchClose{0.08};
// the stretch as the design lays it, how far the points are from it
std::optional<trace_stretch> stretch_of(design const &Design, std::vector<glm::dvec2> const &Points)
{
auto const laid{compute(Design)};
if (false == laid.valid)
return std::nullopt;
trace_stretch stretch;
stretch.valid = true;
stretch.vertices = Design.vertices;
for (std::size_t k = 0; k < stretch.vertices.size() && k < laid.vertices.size(); ++k)
stretch.vertices[k].position = laid.vertices[k];
double sum{0.0};
for (auto const &projection : project(laid, Points))
{
stretch.offsets.push_back(projection.offset);
sum += projection.offset * projection.offset;
stretch.worst = std::max(stretch.worst, std::abs(projection.offset));
}
stretch.rms = Points.empty() ? 0.0 : std::sqrt(sum / static_cast<double>(Points.size()));
return stretch;
}
// how far the points are from the line the design lays, a point clicked by mistake far off pulling it less than the square of its
// departure would
bool stretch_residuals(design Trial, std::vector<glm::dvec2> const &Points, std::vector<double> &Residuals)
{
Trial.norms.checked = false;
auto const laid{compute(Trial)};
if (false == laid.valid)
return false;
auto const line{sample_plan(laid, 0.0, laid.length, std::clamp(laid.length / 300.0, 0.5, 4.0))};
Residuals.clear();
for (auto const &point : Points)
{
auto const offset{project_on(line, point).offset};
auto const departure{std::abs(offset)};
Residuals.push_back(departure <= kTraceRobust ? offset : std::copysign(std::sqrt(2.0 * kTraceRobust * departure - kTraceRobust * kTraceRobust), offset));
}
return true;
}
// the same for a design of one compound curve, its shape made straight away as fit_between makes it, many times quicker than the whole
// design laid: the fits of compound curves try it some thousand times
bool compound_residuals(design const &Trial, std::vector<glm::dvec2> const &Points, std::vector<double> &Residuals)
{
if (Trial.vertices.size() != 1 || false == Trial.vertices.front().compound || Trial.vertices.front().arcs.empty())
return stretch_residuals(Trial, Points, Residuals);
glm::dvec2 corner;
if (false == tangent_intersection(Trial, corner))
return false;
auto const start{plan_of(Trial.start)};
auto const end{plan_of(Trial.end)};
auto const in{glm::normalize(Trial.start_direction)};
auto const out{glm::normalize(Trial.end_direction)};
auto const turn{cross(in, out)};
auto const deflection{std::atan2(std::abs(turn), glm::dot(in, out))};
if (deflection < 1e-7 || deflection > kPi - 1e-3)
return false;
int const side{turn > 0.0 ? 1 : -1};
auto const &vertex{Trial.vertices.front()};
std::vector<compound_arc> arcs{{std::max(1.0, vertex.radius), 0.0, vertex.share}};
for (auto const &arc : vertex.arcs)
arcs.push_back({std::max(1.0, arc.radius), std::max(0.0, arc.transition), arc.share});
auto const shape{build_compound(deflection, arcs, std::max(0.0, vertex.transition_in), std::max(0.0, vertex.transition_out))};
auto const normalin{perpendicular(in) * static_cast<double>(side)};
auto const offset{in * shape.end.x + normalin * shape.end.y};
auto const along{cross(out, offset) / cross(out, in)};
auto const after{glm::dot(offset - in * along, out)};
if (along <= 0.0 || after <= 0.0)
return false;
// a curve too large for the straights is made smaller as a whole, its radii and its transitions alike, as fit_between does it
auto const scale{std::min({1.0, (glm::distance(corner, start) + std::max(0.0, Trial.start_reserve)) / along, (glm::distance(end, corner) + std::max(0.0, Trial.end_reserve)) / after})};
// the curve in chords of 4 m at most, the straights either side carried on far
constexpr double kFar{5000.0};
auto const begin{corner - in * along * scale};
auto origin{glm::dvec2{0.0}};
auto heading{in};
std::vector<glm::dvec2> line{begin - in * kFar, begin};
for (auto const &segment : shape.segments)
{
auto const normal{perpendicular(heading) * static_cast<double>(side)};
auto const steps{std::max(1, static_cast<int>(std::ceil(segment[2] * scale / 4.0)))};
local_point local;
for (int i = 1; i <= steps; ++i)
{
local = spiral_point(segment[0], segment[1], segment[2], segment[2] * i / steps);
line.push_back(begin + (origin + heading * local.x + normal * local.y) * scale);
}
origin += heading * local.x + normal * local.y;
heading = turned(heading, side * local.angle);
}
line.push_back(line.back() + out * kFar);
plan_line plan;
plan.points = std::move(line);
plan.chainages.assign(plan.points.size(), 0.0);
Residuals.clear();
for (auto const &point : Points)
{
auto const offset{project_on(plan, point).offset};
auto const departure{std::abs(offset)};
Residuals.push_back(departure <= kTraceRobust ? offset : std::copysign(std::sqrt(2.0 * kTraceRobust * departure - kTraceRobust * kTraceRobust), offset));
}
return true;
}
// the longest transition a fit gives an arc of the radius at the speed: half as long again as the norms ask for, 30 m at least; one
// longer only keeps to points clicked a little off better, not to the track
double longest_transition(double const Speed, double const Radius, limits const &Norms)
{
auto const allowed{std::max(1.0, std::min(std::max(1.0, Speed), speed_limit(Radius, Norms)))};
return std::max(30.0, 1.5 * recommend(allowed, Radius, Norms).transition);
}
// a curve of one radius with transitions of the length: the cant of the norms for it, as much as the transitions ramp
void plain_curve(vertex &Vertex, double const Radius, double const Transition, double const Speed, limits const &Norms)
{
Vertex.radius = Radius;
Vertex.compound = false;
Vertex.share = 1.0;
Vertex.arcs.clear();
Vertex.spirals = false;
Vertex.transition_in = Vertex.transition_out = Transition;
auto const allowed{std::max(1.0, std::min(Speed, speed_limit(Radius, Norms)))};
Vertex.cant = Transition > 0.0 ? ramped_cant(recommend(allowed, Radius, Norms).cant, Transition, allowed, Norms) : 0.0;
}
// the radius and the length of the transitions of the curve of the vertex following the points best, the rest of the design kept: the
// transitions of a line on the photo are what they are, the norms may have asked for others
void fit_curve(design &Design, std::size_t const Vertex, std::vector<glm::dvec2> const &Points)
{
auto const speed{std::max(1.0, Design.speed)};
// the transitions together turn no more than the curve: longer ones would only be cut to it
auto const deflection{vertex_deflection(Design, Vertex)};
auto const shaped = [&](std::vector<double> const &Parameters, design &Trial) {
Trial = Design;
auto const radius{std::exp(std::clamp(Parameters[0], std::log(kTraceRadiusMin), std::log(kTraceRadiusMax)))};
plain_curve(Trial.vertices[Vertex], radius, std::min({Parameters[1] * Parameters[1], radius * deflection, longest_transition(speed, radius, Trial.norms)}), speed, Trial.norms);
};
residuals_of const residuals{[&](std::vector<double> const &Parameters, std::vector<double> &Residuals) {
design trial;
shaped(Parameters, trial);
return stretch_residuals(trial, Points, Residuals);
}};
// from the transitions it has and from those of the norms; the square root of the length, as a length of nought would hold the
// step at nought
auto const radius{std::max(kTraceRadiusMin, Design.vertices[Vertex].radius)};
auto const allowed{std::max(1.0, std::min(speed, speed_limit(radius, Design.norms)))};
std::vector<double> best;
auto lowest{std::numeric_limits<double>::max()};
for (auto const length : {std::max(Design.vertices[Vertex].transition_in, 10.0), std::max(recommend(allowed, radius, Design.norms).transition, 10.0)})
{
// started below the longest transition: above it the length is held, the fit no longer moving it
std::vector<double> parameters{std::log(radius), std::sqrt(std::min(length, 0.9 * std::min(radius * deflection, longest_transition(speed, radius, Design.norms))))};
if (auto const cost{least_squares(parameters, residuals)}; cost < lowest)
{
lowest = cost;
best = parameters;
}
}
if (best.empty())
return;
design fitted;
shaped(best, fitted);
auto &curve{fitted.vertices[Vertex]};
plain_curve(curve, std::max(kTraceRadiusMin, std::round(curve.radius)), std::round(curve.transition_in), speed, fitted.norms);
Design = fitted;
}
// the radii and the shares of the compound curve of the vertex together with the length of its transitions, following the points
// best, from the shape it has; Between: the transitions of the norms between the arcs, else the arcs meet with none
void fit_compound(design &Design, std::size_t const Vertex, std::vector<glm::dvec2> const &Points, bool const Between = true)
{
auto const base{Design.vertices[Vertex]};
if (false == base.compound || base.arcs.empty())
return;
auto const speed{std::max(1.0, Design.speed)};
int const radii{static_cast<int>(base.arcs.size()) + 1};
// the transitions into the first arc and out of the last one turn no more than those arcs
auto const deflection{vertex_deflection(Design, Vertex)};
auto const reach = [&](vertex const &Curve) {
auto total{std::max(0.0, Curve.share)};
for (auto const &arc : Curve.arcs)
total += std::max(0.0, arc.share);
auto const first{total > 0.0 ? std::max(0.0, Curve.share) / total : 1.0};
auto const last{total > 0.0 && false == Curve.arcs.empty() ? std::max(0.0, Curve.arcs.back().share) / total : 1.0};
return std::min(Curve.radius * first, Curve.arcs.empty() ? Curve.radius : Curve.arcs.back().radius * last) * deflection;
};
// as traced_shape takes them, the square root of the length of the transitions last
std::vector<double> start{std::log(std::max(kTraceRadiusMin, base.radius))};
for (auto const &arc : base.arcs)
start.push_back(std::log(std::max(kTraceRadiusMin, arc.radius)));
for (auto const &arc : base.arcs)
start.push_back(std::log(std::max(1e-3, arc.share) / std::max(1e-3, base.share)));
auto const shaped = [&](std::vector<double> const &Parameters, design &Trial) {
Trial = Design;
auto &curve{Trial.vertices[Vertex]};
auto shape{Design};
shape.norms.checked = Between;
curve = traced_shape(shape, base, Parameters, radii);
if (false == Between)
for (auto &arc : curve.arcs)
arc.transition = 0.0;
curve.transition_in = curve.transition_out = std::min({Parameters.back() * Parameters.back(), reach(curve), longest_transition(speed, curve.radius, Trial.norms), longest_transition(speed, curve.arcs.empty() ? curve.radius : curve.arcs.back().radius, Trial.norms)});
};
residuals_of const residuals{[&](std::vector<double> const &Parameters, std::vector<double> &Residuals) {
design trial;
shaped(Parameters, trial);
return compound_residuals(trial, Points, Residuals);
}};
std::vector<double> best;
auto lowest{std::numeric_limits<double>::max()};
// from the transitions it has, from half of them and from those of the norms for its sharpest arc
auto sharpest{base.radius};
for (auto const &arc : base.arcs)
sharpest = std::min(sharpest, arc.radius);
auto const allowed{std::max(1.0, std::min(speed, speed_limit(sharpest, Design.norms)))};
for (auto const length : {base.transition_in, 0.5 * base.transition_in, recommend(allowed, sharpest, Design.norms).transition})
{
auto parameters{start};
parameters.push_back(std::sqrt(std::max(std::min(length, 0.9 * std::min({reach(base), longest_transition(speed, base.radius, Design.norms), longest_transition(speed, base.arcs.back().radius, Design.norms)})), 10.0)));
if (auto const cost{least_squares(parameters, residuals)}; cost < lowest)
{
lowest = cost;
best = parameters;
}
}
if (best.empty())
return;
design fitted;
shaped(best, fitted);
auto &curve{fitted.vertices[Vertex]};
curve.radius = std::max(kTraceRadiusMin, std::round(curve.radius));
auto smallest{curve.radius};
for (auto &arc : curve.arcs)
smallest = std::min(smallest, arc.radius = std::max(kTraceRadiusMin, std::round(arc.radius)));
curve.transition_in = curve.transition_out = std::round(curve.transition_in);
auto const limit{std::max(1.0, std::min(speed, speed_limit(smallest, fitted.norms)))};
curve.cant = curve.transition_in > 0.0 ? ramped_cant(recommend(limit, smallest, fitted.norms).cant, curve.transition_in, limit, fitted.norms) : 0.0;
if (Between)
recommend_between(curve, speed, fitted.norms);
Design = fitted;
}
// the one curve of the design as a compound curve of more arcs than it has, up to Most, as few as keep within kPointsWithin of the
// points, all along with the transitions of the norms between the arcs: each count started from the curve fitted with an arc less,
// its longest arc cut in two, and from the curvature the points show, the better fit taken
std::optional<trace_stretch> fit_arcs(design const &Single, std::vector<glm::dvec2> const &Points, int const Most)
{
auto const laid{compute(Single)};
if (false == laid.valid || Points.size() < 3)
return std::nullopt;
auto from{laid.length};
auto to{0.0};
for (auto const &e : laid.elements)
if (e.vertex == 0)
{
from = std::min(from, e.chainage);
to = std::max(to, e.chainage + e.length);
}
if (from >= to)
return std::nullopt;
// the curvature at each point of the circle through it and the points either side, at the chainage of the point on the curve
auto const projections{project(laid, Points)};
std::vector<std::pair<double, double>> curvatures;
for (std::size_t i = 1; i + 1 < Points.size(); ++i)
{
auto const a{glm::distance(Points[i - 1], Points[i])};
auto const b{glm::distance(Points[i], Points[i + 1])};
auto const c{glm::distance(Points[i - 1], Points[i + 1])};
if (a * b * c < 1e-9)
continue;
curvatures.emplace_back(projections[i].chainage, 2.0 * std::abs(cross(Points[i] - Points[i - 1], Points[i + 1] - Points[i - 1])) / (a * b * c));
}
auto const &single{Single.vertices.front()};
// the arcs of a curve as a list, the first one too, and back
auto const arcs_of = [](vertex const &Curve) {
std::vector<compound_arc> arcs{{Curve.radius, 0.0, Curve.compound ? Curve.share : 1.0}};
if (Curve.compound)
arcs.insert(arcs.end(), Curve.arcs.begin(), Curve.arcs.end());
return arcs;
};
auto const shaped_of = [&](std::vector<compound_arc> const &Arcs) {
auto shaped{Single};
auto &curve{shaped.vertices.front()};
curve.compound = true;
curve.spirals = false;
curve.radius = Arcs.front().radius;
curve.share = Arcs.front().share;
curve.arcs.assign(Arcs.begin() + 1, Arcs.end());
return shaped;
};
// a curve with the transitions of the norms between its arcs fitted from the shape given, all along with them; none where an
// arc came out too short to be one, a kink the points made
auto const fitted = [&](design Shaped) -> std::optional<std::pair<design, trace_stretch>> {
recommend_between(Shaped.vertices.front(), Single.speed, Single.norms);
fit_compound(Shaped, 0, Points);
auto const laid{compute(Shaped)};
if (std::any_of(laid.elements.begin(), laid.elements.end(), [](element const &Element) {
return Element.vertex == 0 && Element.curvature_start == Element.curvature_end && Element.curvature_start > 0.0 && Element.length < 10.0;
}))
return std::nullopt;
auto const stretch{stretch_of(Shaped, Points)};
if (false == stretch.has_value())
return std::nullopt;
return std::make_pair(Shaped, *stretch);
};
std::vector<trace_stretch> counts; // the fit of each count of arcs, the fewest first
auto previous{Single};
for (auto count{arcs_of(single).size() + 1}; count <= static_cast<std::size_t>(Most); ++count)
{
// from the curve fitted with one arc less, its longest arc cut in two, one a little sharper than the other
auto arcs{arcs_of(previous.vertices.front())};
auto longest{arcs.begin()};
for (auto arc{arcs.begin()}; arc != arcs.end(); ++arc)
if (arc->share * arc->radius > longest->share * longest->radius)
longest = arc;
auto const cut{*longest};
*longest = {cut.radius * 0.9, cut.transition, cut.share * 0.5};
arcs.insert(std::next(longest), {cut.radius * 1.1, 0.0, cut.share * 0.5});
auto choice{fitted(shaped_of(arcs))};
// and from the curvature the points show, each arc along its share of the curve
std::vector<compound_arc> shown;
auto const span{(to - from) / count};
for (std::size_t j = 0; j < count; ++j)
{
double sum{0.0};
int points{0};
for (auto const &[chainage, curvature] : curvatures)
if (chainage >= from + span * j && chainage < from + span * (j + 1))
{
sum += curvature;
++points;
}
auto const curvature{points > 0 ? sum / points : 1.0 / std::max(kTraceRadiusMin, single.radius)};
shown.push_back({std::clamp(1.0 / std::max(curvature, 1.0 / kTraceRadiusMax), kTraceRadiusMin, kTraceRadiusMax), 0.0, span * std::max(curvature, 1e-6)});
}
if (auto other{fitted(shaped_of(shown))}; other.has_value() && (false == choice.has_value() || other->second.worst < choice->second.worst))
choice = other;
if (false == choice.has_value())
continue;
previous = choice->first;
counts.push_back(choice->second);
if (choice->second.worst <= kPointsWithin)
break;
}
if (counts.empty())
return std::nullopt;
// the fewest arcs within kPointsWithin; where none keeps within it, the fewest no more than 0.1 m further off than the closest:
// more arcs only to follow points clicked a little off aren't worth it
auto closest{counts.front().worst};
for (auto const &fit : counts)
closest = std::min(closest, fit.worst);
for (auto const &fit : counts)
if (fit.worst <= std::max(kPointsWithin, closest + 0.1))
return fit;
return counts.back();
}
} // namespace
trace_stretch fit_trace_stretch(design const &Base, std::vector<glm::dvec2> const &Points, int const Radii, bool const Straights)
{
trace_stretch chosen;
if (Points.empty() || glm::length(Base.start_direction) < 1e-9 || glm::length(Base.end_direction) < 1e-9)
return chosen;
auto const speed{std::max(1.0, Base.speed)};
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)};
// a shape with more curves is taken where it follows the points much closer than the simpler one, which doesn't keep to them
auto const take = [&](std::optional<trace_stretch> const &Candidate) {
if (false == Candidate.has_value())
return;
if (false == chosen.valid || (chosen.rms > kStretchClose && Candidate->rms < 0.6 * chosen.rms))
chosen = *Candidate;
};
// one straight, where the two are one
if (auto const offset{end - start}; glm::dot(startdirection, enddirection) > std::cos(2e-3) && std::abs(cross(startdirection, offset)) < 0.25 && glm::dot(startdirection, offset) > 0.0)
{
auto straight{Base};
straight.vertices.clear();
auto const along{glm::dot(startdirection, offset)};
straight.end = {Base.start.x + startdirection.x * along, Base.end.y, Base.start.z + startdirection.y * along};
straight.end_direction = startdirection;
take(stretch_of(straight, Points));
}
// one curve where the straights meet: the radius of a plain arc first, then the radius and the transitions together; the radii of
// a compound curve where one radius doesn't keep to the points, or where they're asked for
std::optional<trace_stretch> one;
if (glm::dvec2 meeting; tangent_intersection(Base, meeting))
{
auto single{Base};
single.vertices.assign(1, vertex{});
auto &curve{single.vertices.front()};
curve.transition_in = curve.transition_out = curve.cant = 0.0;
if (auto const arc{fit_trace(single, 0, Points, 1)}; arc.valid)
{
curve = arc.fitted;
fit_curve(single, 0, Points);
// a fit running off to the largest radius, its transitions eating the arc, is two transitions meeting
auto const &fitted{single.vertices.front()};
auto const runaway{fitted.radius > 0.95 * kTraceRadiusMax};
if (false == runaway)
one = stretch_of(single, Points);
auto spirals{single};
spirals.vertices.front() = {};
spirals.vertices.front().spirals = true;
residuals_of const peak{[&](std::vector<double> const &Parameters, std::vector<double> &Residuals) {
auto trial{spirals};
trial.vertices.front().radius = std::exp(std::clamp(Parameters[0], std::log(kTraceRadiusMin), std::log(kTraceRadiusMax)));
return stretch_residuals(trial, Points, Residuals);
}};
std::vector<double> radius{std::log(std::clamp(arc.fitted.radius, kTraceRadiusMin, kTraceRadiusMax))};
least_squares(radius, peak);
spirals.vertices.front().radius = std::round(std::exp(std::clamp(radius[0], std::log(kTraceRadiusMin), std::log(kTraceRadiusMax))));
if (auto const candidate{stretch_of(spirals, Points)}; candidate.has_value() && (false == one.has_value() || candidate->rms < one->rms))
one = candidate;
// a compound curve is the one curve which keeps to the points the closest: of as few radii as keep within kPointsWithin of
// them, up to three, or up to eight where three don't; fitted with the transitions between the arcs from several starts,
// the problem having more than one bottom
auto const loose = [&]() { return one->rms > kStretchClose || one->worst > kPointsWithin; };
if (one.has_value() && Radii != 1 && (Radii > 1 || loose()))
{
std::optional<trace_stretch> compound;
if (auto const fit{fit_trace(single, 0, Points, Radii)}; fit.valid && fit.fitted.compound)
{
auto several{single};
several.vertices.front() = fit.fitted;
fit_compound(several, 0, Points);
compound = stretch_of(several, Points);
}
if (auto const more{fit_arcs(single, Points, Radii > 1 ? Radii : 8)}; more.has_value() && (false == compound.has_value() || more->worst < compound->worst))
compound = more;
if (compound.has_value() && (Radii > 1 || compound->rms < 0.75 * one->rms || (one->worst > kPointsWithin && compound->worst < one->worst)))
one = compound;
}
}
take(one);
}
// two curves with a straight between them, turning the same way or not: the straight is where the points keep to a line between
// the curves. The parameters: the direction of the straight, its distance from the start across it, the logarithms of the radii,
// first with the transitions of the single curve, then with the square roots of the lengths of the transitions of each curve too
constexpr std::size_t kParameters{4};
if (Straights && Points.size() > kParameters && (false == chosen.valid || chosen.rms > kStretchClose))
{
auto const transition{one.has_value() && false == one->vertices.empty() ? one->vertices.front().transition_in : 0.0};
auto const shaped = [&](std::vector<double> const &Parameters, design &Trial) {
glm::dvec2 const direction{std::cos(Parameters[0]), std::sin(Parameters[0])};
glm::dvec2 const across{-direction.y, direction.x};
auto const meet = [&](glm::dvec2 const &From, glm::dvec2 const &Along, double &Distance) {
auto const denominator{glm::dot(across, Along)};
if (std::abs(denominator) < 1e-9)
return false;
Distance = (Parameters[1] - glm::dot(across, From - start)) / denominator;
return Distance > 0.0;
};
double first;
double second;
if (false == meet(start, startdirection, first) || false == meet(end, -enddirection, second))
return false;
if (glm::dot((end - enddirection * second) - (start + startdirection * first), direction) <= 0.0)
return false;
Trial = Base;
Trial.vertices.assign(2, vertex{});
Trial.vertices[0].offset = first;
Trial.vertices[1].offset = second;
for (std::size_t i = 0; i < 2; ++i)
{
auto const radius{std::exp(std::clamp(Parameters[2 + i], std::log(kTraceRadiusMin), std::log(kTraceRadiusMax)))};
auto const length{Parameters.size() > kParameters ? Parameters[kParameters + i] * Parameters[kParameters + i] : transition};
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<double> const &Parameters, std::vector<double> &Residuals) {
design trial;
return shaped(Parameters, trial) && stretch_residuals(trial, Points, Residuals);
}};
// started from the lines through neighbouring points spread over the stretch, short of the two on the straight at its end
auto const guess{std::log(one.has_value() && false == one->vertices.empty() ? std::max(kTraceRadiusMin, one->vertices.front().radius) : 1000.0)};
auto const travel{end - start};
std::vector<double> best;
auto lowest{std::numeric_limits<double>::max()};
auto const pairs{Points.size() - 2};
auto const starts{std::min<std::size_t>(4, pairs)};
for (std::size_t s = 0; s < starts; ++s)
{
auto const i{starts > 1 ? s * (pairs - 1) / (starts - 1) : 0};
auto along{Points[i + 1] - Points[i]};
if (glm::length(along) < 1e-3)
continue;
if (glm::dot(along, travel) < 0.0)
along = -along;
auto const angle{std::atan2(along.y, along.x)};
glm::dvec2 const across{-std::sin(angle), std::cos(angle)};
std::vector<double> parameters{angle, glm::dot(across, Points[i] - start), guess, guess};
if (auto const cost{least_squares(parameters, residuals)}; cost < lowest)
{
lowest = cost;
best = parameters;
}
}
if (false == best.empty())
{
auto longer{best};
longer.push_back(std::sqrt(std::max(transition, 10.0)));
longer.push_back(longer.back());
if (least_squares(longer, residuals) < lowest)
best = longer;
}
if (design two; false == best.empty() && shaped(best, two))
{
for (auto &curve : two.vertices)
plain_curve(curve, std::max(kTraceRadiusMin, std::round(curve.radius)), std::round(curve.transition_in), speed, two.norms);
// none where a curve ran to the end of the radii tried, turns hardly at all, or doesn't fit between the straights: the
// points made the fit find two curves where they show one
auto const laid{compute(two)};
auto const sound{laid.valid && laid.radius_warnings.empty() && laid.curves.size() == 2 && std::all_of(laid.curves.begin(), laid.curves.end(), [](curve_report const &Curve) {
return Curve.radius > 1.05 * kTraceRadiusMin && Curve.radius < 0.95 * kTraceRadiusMax && Curve.deflection > 0.25 * kPi / 180.0;
})};
// one compound curve rather than two turning the same way with a short straight between them; two such only where the
// straight is long and they keep to the points much closer
auto const candidate{sound ? stretch_of(two, Points) : std::nullopt};
auto const curves{laid_curves(laid)};
auto const same{curves.size() == 2 && curves[0].side == curves[1].side};
if (candidate.has_value() && (false == same || (curves[1].from - curves[0].to >= shortest_between(Base) && (false == chosen.valid || candidate->rms < 0.5 * chosen.rms))))
take(candidate);
}
}
return chosen;
}
stretch_fitter::~stretch_fitter()
{
{
std::lock_guard<std::mutex> lock{m_mutex};
m_stop = true;
}
m_wake.notify_all();
if (m_thread.joinable())
m_thread.join();
}
void stretch_fitter::ask(request Request)
{
{
std::lock_guard<std::mutex> lock{m_mutex};
auto const same = [&](std::vector<double> const &Key) { return Key == Request.key; };
if (std::any_of(m_asked.begin(), m_asked.end(), [&](request const &Asked) { return same(Asked.key); }) || (m_fitting.has_value() && same(m_fitting->key)) ||
std::any_of(m_results.begin(), m_results.end(), [&](auto const &Result) { return same(Result.first); }))
return;
m_asked.push_back(std::move(Request));
// the cursor moves on: what was asked for long ago waits no more
constexpr std::size_t kWaiting{8};
if (m_asked.size() > kWaiting)
m_asked.erase(m_asked.begin());
if (false == m_thread.joinable())
m_thread = std::thread{[this]() { work(); }};
}
m_wake.notify_one();
}
bool stretch_fitter::done(std::vector<double> const &Key, trace_stretch &Fit) const
{
std::lock_guard<std::mutex> lock{m_mutex};
for (auto const &result : m_results)
if (result.first == Key)
{
Fit = result.second;
return true;
}
return false;
}
bool stretch_fitter::latest(std::vector<double> &Key, trace_stretch &Fit) const
{
std::lock_guard<std::mutex> lock{m_mutex};
if (m_results.empty())
return false;
Key = m_results.front().first;
Fit = m_results.front().second;
return true;
}
void stretch_fitter::work()
{
std::unique_lock<std::mutex> lock{m_mutex};
for (;;)
{
m_wake.wait(lock, [this]() { return m_stop || false == m_asked.empty(); });
if (m_stop)
return;
// the one asked for last first: it's what the cursor shows now
m_fitting = std::move(m_asked.back());
m_asked.pop_back();
auto const asked{*m_fitting};
lock.unlock();
auto fit{fit_trace_stretch(asked.base, asked.points, asked.radii, asked.straights)};
lock.lock();
constexpr std::size_t kKept{16};
m_results.insert(m_results.begin(), {asked.key, std::move(fit)});
if (m_results.size() > kKept)
m_results.pop_back();
m_fitting.reset();
}
}
}