use std::cmp::Reverse;
use std::collections::HashMap;
use opencrg::{CrgGrid, SearchHint, Uv, Xy};
use crate::{LaneId, Mesh, MeshSampler, RoadNetwork};
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub enum CrgPurpose {
Elevation,
Friction,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct CrgAlong {
pub s_offset: f64,
pub t_offset: f64,
pub opposite: bool,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct CrgPose {
pub x: f64,
pub y: f64,
pub heading: f64,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[non_exhaustive]
pub enum CrgMode {
Attached(CrgAlong),
Attached0(CrgAlong),
Genuine {
start: CrgPose,
},
Global {
origin: CrgPose,
},
Object(CrgObject),
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct CrgObject {
pub origin: CrgPose,
pub half_length: f64,
pub half_width: f64,
pub hide_road: bool,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct CrgSurface {
pub file: String,
pub purpose: CrgPurpose,
pub mode: CrgMode,
pub z_offset: f64,
pub z_scale: f64,
pub lanes: Vec<LaneId>,
pub(crate) road: Option<Stretch>,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub(crate) struct Stretch {
pub stations: Vec<RefPoint>,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub(crate) struct RefPoint {
pub s: f64,
pub x: f64,
pub y: f64,
pub heading: f64,
pub z: f64,
pub grade: f64,
pub bank: f64,
pub bank_rate: f64,
}
#[derive(Debug, Clone, Copy)]
struct OnRoad {
s: f64,
t: f64,
z: f64,
grade: f64,
bank: f64,
bank_rate: f64,
ds: [f64; 2],
dt: [f64; 2],
}
const FAR: f64 = 2.2;
impl Stretch {
fn locate(&self, x: f64, y: f64, hint: &mut PatchHint) -> Option<OnRoad> {
let st = &self.stations;
let last = st.len().checked_sub(2)?;
let near = hint
.last
.is_some_and(|(px, py)| (px - x).powi(2) + (py - y).powi(2) < FAR * FAR);
let mut i = if near {
hint.station.min(last)
} else {
(0..st.len())
.min_by(|&a, &b| {
let d = |k: usize| (st[k].x - x).powi(2) + (st[k].y - y).powi(2);
d(a).total_cmp(&d(b))
})?
.min(last)
};
let mut came_from = None;
let (f, d) = loop {
let (f, d) = foot(&st[i], &st[i + 1], x, y);
let next = if f < 0.0 && i > 0 {
i - 1
} else if f > 1.0 && i < last {
i + 1
} else {
break (f, d);
};
if came_from == Some(next) {
break (f.clamp(0.0, 1.0), d);
}
came_from = Some(i);
i = next;
};
hint.last = Some((x, y));
hint.station = i;
if !(0.0..=1.0).contains(&f) {
return None;
}
let (a, b) = (&st[i], &st[i + 1]);
let lerp = |p: f64, q: f64| p + (q - p) * f;
let heading = a.heading + wrap(b.heading - a.heading) * f;
let along = [heading.cos(), heading.sin()];
let left = [-along[1], along[0]];
let bank = lerp(a.bank, b.bank);
let t = d / bank.cos();
let curvature = wrap(b.heading - a.heading) / (b.s - a.s);
let stretch = 1.0 - curvature * d;
Some(OnRoad {
s: lerp(a.s, b.s),
t,
z: lerp(a.z, b.z) + t * bank.sin(),
grade: lerp(a.grade, b.grade),
bank,
bank_rate: lerp(a.bank_rate, b.bank_rate),
ds: along.map(|c| c / stretch),
dt: left.map(|c| c / bank.cos()),
})
}
}
fn foot(a: &RefPoint, b: &RefPoint, x: f64, y: f64) -> (f64, f64) {
let length = b.s - a.s;
let turn = wrap(b.heading - a.heading);
let (sin, cos) = a.heading.sin_cos();
let (wx, wy) = (x - a.x, y - a.y);
if turn.abs() < 1e-12 {
return ((wx * cos + wy * sin) / length, wy * cos - wx * sin);
}
let radius = length / turn;
let (cx, cy) = (-radius * sin, radius * cos);
let (vx, vy) = (cx - wx, cy - wy);
let rho = vx.hypot(vy).copysign(radius);
let heading = (-vx / rho).atan2(vy / rho);
(wrap(heading - a.heading) / turn, radius - rho)
}
fn wrap(angle: f64) -> f64 {
use std::f64::consts::{PI, TAU};
angle - TAU * ((angle + PI) / TAU).floor()
}
impl CrgAlong {
fn uv(&self, s: f64, t: f64) -> (Uv, f64) {
let sign = if self.opposite { -1.0 } else { 1.0 };
let uv = Uv {
u: sign * (s - self.s_offset),
v: sign * (t - self.t_offset),
};
(uv, sign)
}
}
#[derive(Debug)]
pub struct RoadSurface<'a> {
net: &'a RoadNetwork,
mesh: MeshSampler<'a>,
grids: Vec<CrgGrid>,
patches: Vec<Option<Patch>>,
by_lane: HashMap<LaneId, Vec<usize>>,
}
#[derive(Debug)]
struct Patch {
grid: usize,
rigid: Option<Rigid>,
}
#[derive(Debug, Clone, Copy)]
struct Rigid {
from: [f64; 2],
to: [f64; 2],
angle: f64,
}
impl Rigid {
fn apply(&self, x: f64, y: f64) -> Xy {
let (sin, cos) = self.angle.sin_cos();
let (dx, dy) = (x - self.from[0], y - self.from[1]);
Xy {
x: cos * dx + sin * dy + self.to[0],
y: -sin * dx + cos * dy + self.to[1],
}
}
fn back(&self, [x, y]: [f64; 2]) -> [f64; 2] {
let (sin, cos) = self.angle.sin_cos();
[cos * x - sin * y, sin * x + cos * y]
}
}
#[derive(Debug, Clone, Default)]
pub struct SurfaceHint {
patches: Vec<PatchHint>,
}
#[derive(Debug, Clone, Default)]
struct PatchHint {
last: Option<(f64, f64)>,
station: usize,
crg: SearchHint,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub struct SurfaceSample {
pub z: f64,
pub normal: [f64; 3],
pub crg_height: Option<f64>,
pub friction: Option<f64>,
}
const STEP: f64 = 1e-3;
impl<'a> RoadSurface<'a> {
pub fn new(
net: &'a RoadNetwork,
mesh: &'a Mesh,
mut load: impl FnMut(&str) -> Option<CrgGrid>,
) -> Self {
let mut grids = Vec::new();
let mut files: HashMap<&str, Option<usize>> = HashMap::new();
let mut patches = Vec::with_capacity(net.crg_surfaces().len());
let mut by_lane: HashMap<LaneId, Vec<usize>> = HashMap::new();
for (i, record) in net.crg_surfaces().iter().enumerate() {
let loaded = *files.entry(record.file.as_str()).or_insert_with(|| {
let grid = load(&record.file)?;
grids.push(grid);
Some(grids.len() - 1)
});
let Some(index) = loaded else {
patches.push(None);
continue;
};
let grid: &CrgGrid = &grids[index];
let rigid = match record.mode {
CrgMode::Genuine { start } => {
let first = Uv {
u: grid.u_range().0,
v: 0.0,
};
let origin = grid.xy_from_uv(first);
Some(Rigid {
from: [start.x, start.y],
to: [origin.x, origin.y],
angle: start.heading - grid.heading_at_uv(first).phi,
})
}
CrgMode::Global { origin } => Some(Rigid {
from: [origin.x, origin.y],
to: [0.0, 0.0],
angle: origin.heading,
}),
CrgMode::Attached(_) | CrgMode::Attached0(_) | CrgMode::Object(_) => None,
};
patches.push(Some(Patch { grid: index, rigid }));
for &lane in &record.lanes {
by_lane.entry(lane).or_default().push(i);
}
}
for list in by_lane.values_mut() {
list.sort_by_key(|&i| Reverse(i));
}
Self {
net,
mesh: mesh.sampler(),
grids,
patches,
by_lane,
}
}
pub fn sample(&self, x: f64, y: f64, hint: &mut SurfaceHint) -> Option<SurfaceSample> {
let (height, normal, lane) = self.mesh.top(x as f32, y as f32)?;
let records = self.net.crg_surfaces();
if hint.patches.len() < records.len() {
hint.patches.resize_with(records.len(), PatchHint::default);
}
let covering: &[usize] = lane
.and_then(|l| self.by_lane.get(&l))
.map_or(&[], Vec::as_slice);
let mut sample = SurfaceSample {
z: f64::from(height),
normal: normal.to_array().map(f64::from),
crg_height: None,
friction: None,
};
let at = |i: usize| {
let patch = self.patches[i].as_ref()?;
Some(At {
record: &records[i],
grid: &self.grids[patch.grid],
rigid: patch.rigid,
})
};
let object = covering.iter().find_map(|&i| {
let at = at(i)?;
let CrgMode::Object(placed) = at.record.mode else {
return None;
};
Some((at.object(placed, x, y)?, placed.hide_road))
});
let hides = object.is_some_and(|(_, hide)| hide);
for &i in covering {
let Some(at) = at(i) else {
continue;
};
let hidden = match at.record.mode {
CrgMode::Object(_) => true,
CrgMode::Attached(_) => hides && at.record.purpose == CrgPurpose::Elevation,
_ => false,
};
if hidden {
continue;
}
let hint = &mut hint.patches[i];
match at.record.purpose {
CrgPurpose::Elevation if sample.crg_height.is_none() => {
if let Some((z, normal, bump)) = at.elevation(x, y, hint) {
sample.z = z;
sample.normal = normal;
sample.crg_height = Some(bump);
}
}
CrgPurpose::Friction if sample.friction.is_none() => {
sample.friction = at.friction(x, y, hint);
}
_ => {}
}
}
if let Some(((z, [dz_dx, dz_dy]), _)) = object {
let [nx, ny, nz] = sample.normal;
sample.z += z;
sample.normal = unit([nx / nz - dz_dx, ny / nz - dz_dy, 1.0]);
sample.crg_height = Some(sample.crg_height.unwrap_or(0.0) + z);
}
Some(sample)
}
}
enum Found {
Along { on: OnRoad, uv: Uv, sign: f64 },
Placed { uv: Uv, rigid: Rigid },
}
struct At<'r> {
record: &'r CrgSurface,
grid: &'r CrgGrid,
rigid: Option<Rigid>,
}
impl At<'_> {
fn find(&self, x: f64, y: f64, hint: &mut PatchHint) -> Option<Found> {
let on = match &self.record.road {
Some(road) => Some(road.locate(x, y, hint)?),
None => None,
};
match (self.record.mode, self.rigid) {
(CrgMode::Attached(along) | CrgMode::Attached0(along), _) => {
let on = on?;
let (uv, sign) = along.uv(on.s, on.t);
Some(Found::Along { on, uv, sign })
}
(_, Some(rigid)) => {
let uv = self
.grid
.uv_from_xy_near(rigid.apply(x, y), &mut hint.crg)?;
Some(Found::Placed { uv, rigid })
}
(_, None) => None,
}
}
fn elevation(&self, x: f64, y: f64, hint: &mut PatchHint) -> Option<(f64, [f64; 3], f64)> {
let grid = self.grid;
let k = self.record.z_scale;
let (z, normal, bump) = match self.find(x, y, hint)? {
Found::Along { on, uv, sign } => {
let bump = grid.grid_at_uv(uv)?;
let (z, dz_ds, dz_dt) = if matches!(self.record.mode, CrgMode::Attached(_)) {
(
on.z + k * bump.z,
on.grade + on.t * on.bank.cos() * on.bank_rate + k * sign * bump.dz_du,
on.bank.sin() + k * sign * bump.dz_dv,
)
} else {
let z = grid.elevation_at_uv(uv)?;
let (dz_du, dz_dv) = slopes(grid, uv, z);
(k * z, k * sign * dz_du, k * sign * dz_dv)
};
let gradient = [0, 1].map(|c| dz_ds * on.ds[c] + dz_dt * on.dt[c]);
(z, unit([-gradient[0], -gradient[1], 1.0]), bump.z)
}
Found::Placed { uv, rigid } => {
let z = grid.elevation_at_uv(uv)?;
let n = grid.normal_at_uv(uv)?;
let [nx, ny] = rigid.back([n.x, n.y]);
let bump = grid.grid_at_uv(uv).map_or(0.0, |g| g.z);
(k * z, unit([k * nx, k * ny, n.z]), bump)
}
};
Some((z + self.record.z_offset, normal, k * bump))
}
fn object(&self, placed: CrgObject, x: f64, y: f64) -> Option<(f64, [f64; 2])> {
let CrgPose {
x: ox,
y: oy,
heading,
} = placed.origin;
let (sin, cos) = heading.sin_cos();
let (dx, dy) = (x - ox, y - oy);
let uv = Uv {
u: cos * dx + sin * dy,
v: -sin * dx + cos * dy,
};
if uv.u.abs() > placed.half_length || uv.v.abs() > placed.half_width {
return None;
}
let z = self.grid.elevation_at_uv(uv)?;
let (dz_du, dz_dv) = slopes(self.grid, uv, z);
let k = self.record.z_scale;
let slope = [cos * dz_du - sin * dz_dv, sin * dz_du + cos * dz_dv];
Some((k * z, slope.map(|d| k * d)))
}
fn friction(&self, x: f64, y: f64, hint: &mut PatchHint) -> Option<f64> {
let uv = match self.find(x, y, hint)? {
Found::Along { uv, .. } | Found::Placed { uv, .. } => uv,
};
self.grid.grid_at_uv(uv).map(|g| g.z)
}
}
fn slopes(grid: &CrgGrid, uv: Uv, z: f64) -> (f64, f64) {
let slope = |du: f64, dv: f64| {
let at = |sign: f64| {
grid.elevation_at_uv(Uv {
u: uv.u + sign * du,
v: uv.v + sign * dv,
})
};
match (at(-1.0), at(1.0)) {
(Some(a), Some(b)) => (b - a) / (2.0 * STEP),
(None, Some(b)) => (b - z) / STEP,
(Some(a), None) => (z - a) / STEP,
(None, None) => 0.0,
}
};
(slope(STEP, 0.0), slope(0.0, STEP))
}
fn unit(v: [f64; 3]) -> [f64; 3] {
let length = v.iter().map(|c| c * c).sum::<f64>().sqrt();
v.map(|c| c / length)
}