use std::f64::consts::{PI, TAU};
use super::{attr_f64, child, MAX_LENGTH};
use crate::road::{eval_poly3, GeomRec, GeomShape};
const STEP: f64 = 0.25;
const TURN: f64 = PI / 32.0;
const MAX_EDGES: f64 = 1024.0;
pub(super) const GAP: f64 = 0.01;
pub(super) struct Curve {
pub points: Vec<[f64; 4]>,
pub gap: f64,
pub end_heading: f64,
pub turned: bool,
}
pub(super) fn sample(
node: roxmltree::Node,
next: Option<roxmltree::Node>,
previous: Option<f64>,
) -> Option<Curve> {
let given = attr_f64(node, "hdg");
let curve = trace(node, next, given, previous)?;
if curve.gap == 0.0 || given.is_none() {
return Some(curve);
}
match previous.and_then(|hdg| trace(node, next, Some(hdg), previous)) {
Some(joined) if joined.gap == 0.0 => Some(Curve {
turned: true,
..joined
}),
_ => Some(curve),
}
}
fn trace(
node: roxmltree::Node,
next: Option<roxmltree::Node>,
hdg: Option<f64>,
previous: Option<f64>,
) -> Option<Curve> {
let corner = |n: roxmltree::Node| -> Option<[f64; 4]> {
let z = attr_f64(n, "z").unwrap_or(0.0);
let height = attr_f64(n, "height").unwrap_or(0.0);
Some([attr_f64(n, "u")?, attr_f64(n, "v")?, z, height])
};
let start = corner(node)?;
let end = next.and_then(corner);
let [u, v, ..] = start;
let given = attr_f64(node, "length").filter(|l| *l > 0.0 && *l <= MAX_LENGTH);
let (path, edges) = if child(node, "line").is_some() {
let hdg = hdg
.or_else(|| end.map(|[eu, ev, ..]| (ev - v).atan2(eu - u)))
.or(previous)
.unwrap_or(0.0);
let length = given
.or_else(|| end.map(|[eu, ev, ..]| (eu - u).hypot(ev - v)))
.unwrap_or(0.0);
let line = GeomRec::new(0.0, u, v, hdg, length, GeomShape::Line);
(Path::Geom(line), 1.0)
} else if let Some(arc) = child(node, "arc") {
let curvature = attr_f64(arc, "curvature")?;
let hdg = hdg.or(previous).unwrap_or(0.0);
let length = given
.or_else(|| {
end.map(|[eu, ev, ..]| round_to(u, v, hdg, curvature, eu, ev))
.filter(|l| *l <= MAX_LENGTH)
})
.unwrap_or(0.0);
let arc = GeomRec::new(0.0, u, v, hdg, length, GeomShape::Arc { curvature });
let edges = (length / STEP).max(curvature.abs() * length / TURN);
(Path::Geom(arc), edges)
} else {
let pp = child(node, "paramPoly3")?;
let coeff = |n: &str| attr_f64(pp, n).unwrap_or(0.0);
let p_max = match pp.attribute("pRange").map(str::to_ascii_lowercase) {
Some(r) if r == "arclength" => given?,
_ => 1.0,
};
let poly = Path::Poly {
origin: (u, v),
hdg: hdg.or(previous).unwrap_or(0.0),
u: [coeff("aU"), coeff("bU"), coeff("cU"), coeff("dU")],
v: [coeff("aV"), coeff("bV"), coeff("cV"), coeff("dV")],
p_max,
};
let estimate = given.unwrap_or_else(|| {
(0..16)
.map(|k| {
let (a, b) = (poly.at(k as f64 / 16.0), poly.at((k + 1) as f64 / 16.0));
(b.0 - a.0).hypot(b.1 - a.1)
})
.sum()
});
(poly, (estimate / STEP).max(8.0))
};
let edges = edges.ceil().clamp(1.0, MAX_EDGES) as usize;
let to = end.unwrap_or(start);
let point = |f: f64| {
let (pu, pv) = path.at(f);
let lerp = |i: usize| start[i] + (to[i] - start[i]) * f;
[pu, pv, lerp(2), lerp(3)]
};
let mut points: Vec<[f64; 4]> = (0..edges).map(|k| point(k as f64 / edges as f64)).collect();
let last = point(1.0);
let gap = end.map_or(0.0, |[eu, ev, ..]| (last[0] - eu).hypot(last[1] - ev));
let runs_on = end.is_none() && (last[0] - u).hypot(last[1] - v) > GAP;
if runs_on || gap > GAP {
points.push(last);
}
Some(Curve {
points,
gap: if gap > GAP { gap } else { 0.0 },
end_heading: path.heading_at_end(),
turned: false,
})
}
enum Path {
Geom(GeomRec),
Poly {
origin: (f64, f64),
hdg: f64,
u: [f64; 4],
v: [f64; 4],
p_max: f64,
},
}
impl Path {
fn heading_at_end(&self) -> f64 {
match self {
Self::Geom(geom) => geom.pose(geom.length).2,
Self::Poly {
hdg, u, v, p_max, ..
} => {
let slope = |c: &[f64; 4]| c[1] + p_max * (2.0 * c[2] + 3.0 * c[3] * p_max);
hdg + slope(v).atan2(slope(u))
}
}
}
fn at(&self, f: f64) -> (f64, f64) {
match self {
Self::Geom(geom) => {
let (u, v, _) = geom.pose(f * geom.length);
(u, v)
}
Self::Poly {
origin,
hdg,
u,
v,
p_max,
} => {
let (sin, cos) = hdg.sin_cos();
let (pu, pv) = (eval_poly3(*u, f * p_max), eval_poly3(*v, f * p_max));
(
origin.0 + pu * cos - pv * sin,
origin.1 + pu * sin + pv * cos,
)
}
}
}
}
fn round_to(u: f64, v: f64, hdg: f64, curvature: f64, to_u: f64, to_v: f64) -> f64 {
if curvature.abs() < 1e-9 {
return (to_u - u).hypot(to_v - v);
}
let (sin, cos) = hdg.sin_cos();
let (cu, cv) = (u - sin / curvature, v + cos / curvature);
let (a, b) = ((u - cu, v - cv), (to_u - cu, to_v - cv));
let turned = (a.0 * b.1 - a.1 * b.0).atan2(a.0 * b.0 + a.1 * b.1) * curvature.signum();
let turned = if turned <= 1e-9 { turned + TAU } else { turned };
turned / curvature.abs()
}