From d9cddc511e67cf2766b7edd03106a3427f122d7b Mon Sep 17 00:00:00 2001 From: Paris DOUADY Date: Wed, 3 Jan 2024 14:59:15 +0100 Subject: [PATCH] max steepness as calculated by point generation is now used to check when building --- geom/src/boldspline.rs | 3 +- geom/src/lib.rs | 6 +- geom/src/{splines.rs => spline.rs} | 0 geom/src/spline1.rs | 178 +++++++++++++++++++++++++++++ native_app/src/game_loop.rs | 6 +- native_app/src/gui/roadbuild.rs | 175 +++++++++++++--------------- simulation/src/map/map.rs | 35 +++++- simulation/src/map/mod.rs | 1 + simulation/src/map/objects/road.rs | 127 ++++++++++++-------- 9 files changed, 380 insertions(+), 151 deletions(-) rename geom/src/{splines.rs => spline.rs} (100%) create mode 100644 geom/src/spline1.rs diff --git a/geom/src/boldspline.rs b/geom/src/boldspline.rs index db667100..7f5455ca 100644 --- a/geom/src/boldspline.rs +++ b/geom/src/boldspline.rs @@ -1,4 +1,4 @@ -use super::Vec2; +use super::{Vec2, Vec3}; use crate::aabb::AABB; use crate::{vec2, BoldLine, Circle, Intersect, Polygon, Segment, Shape, Spline, OBB}; use serde::{Deserialize, Serialize}; @@ -82,6 +82,7 @@ impl Intersect for BoldSpline { } } +pub static mut DEBUG_POS: Vec = Vec::new(); pub static mut DEBUG_OBBS: Vec = Vec::new(); pub static mut DEBUG_SPLINES: Vec = Vec::new(); diff --git a/geom/src/lib.rs b/geom/src/lib.rs index d2df887d..fe9735c3 100644 --- a/geom/src/lib.rs +++ b/geom/src/lib.rs @@ -51,8 +51,9 @@ mod segment; mod segment3; pub mod skeleton; mod sphere; +mod spline; +mod spline1; mod spline3; -mod splines; mod transform; mod v2; mod v3; @@ -85,8 +86,9 @@ pub use ray3::*; pub use segment::*; pub use segment3::*; pub use sphere::*; +pub use spline::*; +pub use spline1::*; pub use spline3::*; -pub use splines::*; pub use transform::*; pub use v2::*; pub use v3::*; diff --git a/geom/src/splines.rs b/geom/src/spline.rs similarity index 100% rename from geom/src/splines.rs rename to geom/src/spline.rs diff --git a/geom/src/spline1.rs b/geom/src/spline1.rs new file mode 100644 index 00000000..3542e5f6 --- /dev/null +++ b/geom/src/spline1.rs @@ -0,0 +1,178 @@ +use serde::{Deserialize, Serialize}; + +#[derive(Copy, Clone, Debug, Serialize, Deserialize)] +pub struct Spline1 { + pub from: f32, + pub to: f32, + pub from_derivative: f32, + pub to_derivative: f32, +} + +impl Default for Spline1 { + fn default() -> Self { + Self { + from: 0.0, + to: 0.0, + from_derivative: 0.0, + to_derivative: 0.0, + } + } +} + +impl Spline1 { + #[inline] + pub fn get(&self, t: f32) -> f32 { + (1.0 - t).powi(3) * self.from + + 3.0 * t * (1.0 - t).powi(2) * (self.from + self.from_derivative) + + 3.0 * t.powi(2) * (1.0 - t) * (self.to - self.to_derivative) + + t.powi(3) * self.to + } + + #[inline] + pub fn derivative(&self, t: f32) -> f32 { + -3.0 * (t - 1.0).powi(2) * self.from + + 3.0 * (t - 1.0) * (3.0 * t - 1.0) * (self.from + self.from_derivative) + + 3.0 * t * (2.0 - 3.0 * t) * (self.to - self.to_derivative) + + 3.0 * t.powi(2) * self.to + } + + #[inline] + pub fn derivative_2(&self, t: f32) -> f32 { + 6.0 * (1.0 - t) * self.from + + (18.0 * t - 12.0) * (self.from + self.from_derivative) + + (6.0 - 18.0 * t) * (self.to - self.to_derivative) + + 6.0 * t * self.to + } + + #[allow(non_snake_case)] + pub fn split_at(&self, t: f32) -> (Spline1, Spline1) { + // https://upload.wikimedia.org/wikipedia/commons/1/11/Bezier_rec.png + let mid = self.get(t); + let H = (self.to - self.to_derivative) * t + (self.from + self.from_derivative) * (1.0 - t); + + let L2 = self.from + self.from_derivative * t; + let L3 = L2 + (H - L2) * t; + + let from_spline = Spline1 { + from: self.from, + to: mid, + from_derivative: L2 - self.from, + to_derivative: mid - L3, + }; + + let R3 = self.to - self.to_derivative * (1.0 - t); + let R2 = R3 + (H - R3) * (1.0 - t); + + let to_spline = Spline1 { + from: mid, + to: self.to, + from_derivative: R2 - mid, + to_derivative: self.to - R3, + }; + + (from_spline, to_spline) + } + + pub fn smart_points( + &self, + detail: f32, + start: f32, + end: f32, + ) -> impl Iterator + '_ { + self.smart_points_t(detail, start, end) + .map(move |t| self.get(t)) + } + + pub fn smart_points_t( + &self, + detail: f32, + start: f32, + end: f32, + ) -> impl Iterator + '_ { + let detail = detail.abs(); + + std::iter::once(start) + .chain(SmartPoints1 { + spline: self, + t: start, + end, + detail, + }) + .chain(std::iter::once(end)) + } + + pub fn into_smart_points_t( + self, + detail: f32, + start: f32, + end: f32, + ) -> impl Iterator { + let detail = detail.abs(); + + std::iter::once(start) + .chain(OwnedSmartPoints1 { + spline: self, + t: start, + end, + detail, + }) + .chain(std::iter::once(end)) + } + + pub fn into_smart_points(self, detail: f32, start: f32, end: f32) -> impl Iterator { + self.into_smart_points_t(detail, start, end) + .map(move |t| self.get(t)) + } + + pub fn points(&self, n: usize) -> impl Iterator + '_ { + (0..n).map(move |i| { + let c = i as f32 / (n - 1) as f32; + + self.get(c) + }) + } + + #[inline] + fn step(&self, t: f32, detail: f32) -> f32 { + let dot = self.derivative(t).abs(); + (detail / dot).min(0.15) + } +} + +pub struct SmartPoints1<'a> { + spline: &'a Spline1, + t: f32, + end: f32, + detail: f32, +} + +impl<'a> Iterator for SmartPoints1<'a> { + type Item = f32; + + fn next(&mut self) -> Option { + self.t += self.spline.step(self.t, self.detail); + if self.t > self.end { + return None; + } + Some(self.t) + } +} + +pub struct OwnedSmartPoints1 { + spline: Spline1, + t: f32, + end: f32, + detail: f32, +} + +impl Iterator for OwnedSmartPoints1 { + type Item = f32; + + fn next(&mut self) -> Option { + self.t += self.spline.step(self.t, self.detail); + if self.t > self.end { + return None; + } + Some(self.t) + } +} diff --git a/native_app/src/game_loop.rs b/native_app/src/game_loop.rs index 8e31373c..a7d691e4 100644 --- a/native_app/src/game_loop.rs +++ b/native_app/src/game_loop.rs @@ -319,6 +319,9 @@ impl State { let mut col = LinearColor::WHITE; col.a = 0.2; unsafe { + for v in &geom::DEBUG_POS { + immediate.circle(*v, 1.0).color(LinearColor::RED); + } for v in &geom::DEBUG_OBBS { immediate .obb(*v, map.environment.height(v.center()).unwrap_or(0.0) + 8.0) @@ -335,8 +338,9 @@ impl State { ) .color(col); } - //geom::DEBUG_OBBS.clear(); + geom::DEBUG_OBBS.clear(); geom::DEBUG_SPLINES.clear(); + geom::DEBUG_POS.clear(); } immediate.apply(&mut self.immtess, ctx); diff --git a/native_app/src/gui/roadbuild.rs b/native_app/src/gui/roadbuild.rs index c20376bf..8f78ba60 100644 --- a/native_app/src/gui/roadbuild.rs +++ b/native_app/src/gui/roadbuild.rs @@ -1,35 +1,28 @@ -use crate::gui::{PotentialCommands, Tool}; -use crate::inputmap::{InputAction, InputMap}; -use crate::rendering::immediate::{ImmediateDraw, ImmediateSound}; -use crate::uiworld::UiWorld; use common::AudioKind; use geom::{BoldLine, BoldSpline, Camera, PolyLine, ShapeEnum, Spline}; -use geom::{PolyLine3, Spline3, Vec2, Vec3}; +use geom::{PolyLine3, Vec2, Vec3}; use simulation::map::{ - Intersection, LanePatternBuilder, Map, MapProject, ProjectFilter, ProjectKind, PylonPosition, - RoadSegmentKind, + LanePatternBuilder, Map, MapProject, ProjectFilter, ProjectKind, PylonPosition, RoadSegmentKind, }; use simulation::world_command::{WorldCommand, WorldCommands}; use simulation::Simulation; -use BuildState::{Hover, Interpolation, Start}; +use BuildState::{Hover, Interpolation, Start, StartInterp}; use ProjectKind::{Building, Ground, Inter, Road}; +use crate::gui::{PotentialCommands, Tool}; +use crate::inputmap::{InputAction, InputMap}; +use crate::rendering::immediate::{ImmediateDraw, ImmediateSound}; +use crate::uiworld::UiWorld; + #[derive(Copy, Clone, Debug, Default)] pub enum BuildState { #[default] Hover, Start(MapProject), + StartInterp(MapProject), Interpolation(Vec2, MapProject), } -#[derive(Default)] -pub struct RoadBuildResource { - pub build_state: BuildState, - pub pattern_builder: LanePatternBuilder, - pub snap_to_grid: bool, - pub height_offset: f32, -} - /// Road building tool /// Allows to build roads and intersections pub fn roadbuild(sim: &Simulation, uiworld: &mut UiWorld) { @@ -158,9 +151,9 @@ pub fn roadbuild(sim: &Simulation, uiworld: &mut UiWorld) { let is_rail = state.pattern_builder.rail; - let is_valid = match (state.build_state, cur_proj.kind) { + let mut is_valid = match (state.build_state, cur_proj.kind) { (Hover, Building(_)) => false, - (Start(selected_proj), _) => { + (Start(selected_proj) | StartInterp(selected_proj), _) => { let sp = BoldLine::new( PolyLine::new(vec![selected_proj.pos.xy(), cur_proj.pos.xy()]), patwidth * 0.5, @@ -201,26 +194,55 @@ pub fn roadbuild(sim: &Simulation, uiworld: &mut UiWorld) { _ => true, }; - state.update_drawing(map, immdraw, cur_proj, patwidth, tool, is_valid); + let build_args = match state.build_state { + StartInterp(selected_proj) if !cur_proj.is_ground() => { + Some((selected_proj, None, state.pattern_builder.build())) + } + Start(selected_proj) => Some((selected_proj, None, state.pattern_builder.build())), + Interpolation(interpoint, selected_proj) => { + let inter = Some(interpoint); + Some((selected_proj, inter, state.pattern_builder.build())) + } + _ => None, + }; potential_command.0.clear(); - match state.build_state { - Hover => {} - Start(selected_proj) => potential_command.set(WorldCommand::MapMakeConnection { + + let mut points = None; + + if let Some((selected_proj, inter, pat)) = build_args { + potential_command.set(WorldCommand::MapMakeConnection { from: selected_proj, to: cur_proj, - inter: None, - pat: state.pattern_builder.build(), - }), - Interpolation(interpoint, selected_proj) => { - potential_command.set(WorldCommand::MapMakeConnection { - from: selected_proj, - to: cur_proj, - inter: Some(interpoint), - pat: state.pattern_builder.build(), - }) - } + inter, + pat, + }); + + let connection_segment = match inter { + Some(x) => RoadSegmentKind::from_elbow(selected_proj.pos.xy(), cur_proj.pos.xy(), x), + None => RoadSegmentKind::Straight, + }; + + points = match simulation::map::Road::generate_points( + selected_proj.pos, + cur_proj.pos, + connection_segment, + is_rail, + &map.environment, + ) { + Ok(p) => Some(p), + Err((p, _err)) => { + is_valid = false; + if let Some(p) = p { + Some(p) + } else { + None + } + } + }; } + state.update_drawing(map, immdraw, cur_proj, patwidth, is_valid, points); + if is_valid && inp.just_act.contains(&InputAction::Select) { log::info!( "left clicked with state {:?} and {:?}", @@ -228,16 +250,20 @@ pub fn roadbuild(sim: &Simulation, uiworld: &mut UiWorld) { cur_proj.kind ); - match (state.build_state, cur_proj.kind, tool) { - (Hover, Ground, _) | (Hover, Road(_), _) | (Hover, Inter(_), _) => { + match (state.build_state, cur_proj.kind) { + (Hover, Ground) | (Hover, Road(_)) | (Hover, Inter(_)) => { // Hover selection - state.build_state = Start(cur_proj); + if tool == Tool::RoadbuildCurved { + state.build_state = StartInterp(cur_proj); + } else { + state.build_state = Start(cur_proj); + } } - (Start(v), Ground, Tool::RoadbuildCurved) => { + (StartInterp(v), Ground) => { // Set interpolation point state.build_state = Interpolation(mousepos.xy(), v); } - (Start(_), _, _) => { + (Start(_) | StartInterp(_), _) => { // Straight connection to something immsound.play("road_lay", AudioKind::Ui); if let Some(wc) = potential_command.0.drain(..).next() { @@ -246,7 +272,7 @@ pub fn roadbuild(sim: &Simulation, uiworld: &mut UiWorld) { state.build_state = Hover; } - (Interpolation(_, _), _, _) => { + (Interpolation(_, _), _) => { // Interpolated connection to something immsound.play("road_lay", AudioKind::Ui); if let Some(wc) = potential_command.0.drain(..).next() { @@ -260,6 +286,14 @@ pub fn roadbuild(sim: &Simulation, uiworld: &mut UiWorld) { } } +#[derive(Default)] +pub struct RoadBuildResource { + pub build_state: BuildState, + pub pattern_builder: LanePatternBuilder, + pub snap_to_grid: bool, + pub height_offset: f32, +} + fn check_angle(map: &Map, from: MapProject, to: Vec2, is_rail: bool) -> bool { let max_turn_angle = if is_rail { 1.0 * std::f32::consts::PI / 180.0 @@ -357,8 +391,8 @@ impl RoadBuildResource { immdraw: &mut ImmediateDraw, proj: MapProject, patwidth: f32, - tool: Tool, is_valid: bool, + points: Option, ) { let mut proj_pos = proj.pos; proj_pos.z += 0.1; @@ -368,24 +402,12 @@ impl RoadBuildResource { simulation::config().gui_danger }; - let interf = |ang: Vec2, proj: MapProject| match proj.kind { - Inter(i) => map - .intersections() - .get(i) - .map(|i| i.interface_at(map.roads(), patwidth, ang)) - .unwrap_or_default(), - Road(_) => Intersection::empty_interface(patwidth), - Building(_) => 0.0, - ProjectKind::Lot(_) => 0.0, - Ground => Intersection::empty_interface(patwidth), - }; - let p = match self.build_state { Hover => { immdraw.circle(proj_pos, patwidth * 0.5).color(col); return; } - Start(x) if matches!(tool, Tool::RoadbuildCurved) && proj.kind.is_ground() => { + StartInterp(x) if proj.kind.is_ground() => { let dir = unwrap_or!((proj_pos - x.pos).try_normalize(), { immdraw.circle(proj_pos, patwidth * 0.5).color(col); return; @@ -406,48 +428,7 @@ impl RoadBuildResource { return; } - Start(x) => { - immdraw.circle(proj_pos, patwidth * 0.5).color(col); - immdraw.circle(x.pos.up(0.1), patwidth * 0.5).color(col); - - let Ok(points) = simulation::map::Road::generate_points( - x.pos, - proj_pos, - RoadSegmentKind::Straight, - false, - &map.environment, - ) else { - // todo: handle error - return; - }; - - immdraw - .polyline(points.into_vec(), patwidth, false) - .color(col); - - let istart = interf((proj_pos - x.pos).xy().normalize(), x); - let iend = interf(-(proj_pos - x.pos).xy().normalize(), proj); - PolyLine3::new(vec![x.pos.up(0.1), proj_pos]).cut(istart, iend) - } - Interpolation(p, x) => { - let sp = Spline3 { - from: x.pos.up(0.1), - to: proj_pos, - from_derivative: (p - x.pos.xy()).z0() * std::f32::consts::FRAC_1_SQRT_2, - to_derivative: (proj_pos.xy() - p).z0() * std::f32::consts::FRAC_1_SQRT_2, - }; - let points: Vec<_> = sp.smart_points(1.0, 0.0, 1.0).collect(); - - immdraw.polyline(&*points, patwidth, false).color(col); - - immdraw.circle(sp.get(0.0), patwidth * 0.5).color(col); - immdraw.circle(sp.get(1.0), patwidth * 0.5).color(col); - - let istart = interf((p - x.pos.xy()).normalize(), x); - let iend = interf(-(proj_pos.xy() - p).normalize(), proj); - - PolyLine3::new(points).cut(istart, iend) - } + _ => unwrap_ret!(points), }; for PylonPosition { @@ -460,5 +441,9 @@ impl RoadBuildResource { .circle(pos.xy().z(terrain_height + 0.1), patwidth * 0.5) .color(col); } + + immdraw.circle(p.first(), patwidth * 0.5).color(col); + immdraw.circle(p.last(), patwidth * 0.5).color(col); + immdraw.polyline(p.into_vec(), patwidth, false).color(col); } } diff --git a/simulation/src/map/map.rs b/simulation/src/map/map.rs index a4a60443..c3de25dd 100644 --- a/simulation/src/map/map.rs +++ b/simulation/src/map/map.rs @@ -157,9 +157,18 @@ impl Map { }; let from_id = mk_inter(from)?; - let to_id = mk_inter(to)?; + let Some(to_id) = mk_inter(to) else { + self.invalidate(from_id); + self.check_invariants(); + return None; + }; - let r = self.connect(from_id, to_id, pattern, connection_segment)?; + let Some(r) = self.connect(from_id, to_id, pattern, connection_segment) else { + self.invalidate(from_id); + self.invalidate(to_id); + self.check_invariants(); + return None; + }; info!( "connect {:?}({:?}) {:?}({:?}) {:?} {:?}: {:?}", @@ -548,7 +557,7 @@ impl Map { &mut self.lanes, &mut self.parking, &mut self.spatial_map, - )?; + ); #[allow(clippy::indexing_slicing)] let r = &self.roads[id]; @@ -747,8 +756,18 @@ impl Map { for turn in inter.turns() { log::debug!("{:?}", turn.id); assert_eq!(turn.id.parent, inter.id); - assert!(self.lanes.contains_key(turn.id.src)); - assert!(self.lanes.contains_key(turn.id.dst)); + assert!( + self.lanes.contains_key(turn.id.src), + "{:?} {:?}", + inter.id, + turn.id.src + ); + assert!( + self.lanes.contains_key(turn.id.dst), + "{:?} {:?}", + inter.id, + turn.id.dst + ); assert!(turn.points.n_points() >= 2); } @@ -860,6 +879,8 @@ impl Map { } assert!(self.parking.reuse_spot.is_empty()); + + log::info!("invariants checked"); } } @@ -870,4 +891,8 @@ impl MapProject { kind: ProjectKind::Ground, } } + + pub fn is_ground(&self) -> bool { + matches!(self.kind, ProjectKind::Ground) + } } diff --git a/simulation/src/map/mod.rs b/simulation/src/map/mod.rs index 8ea72fec..c4b2127a 100644 --- a/simulation/src/map/mod.rs +++ b/simulation/src/map/mod.rs @@ -54,3 +54,4 @@ pub use ::pathfinding as pathfinding_crate; pub const CROSSWALK_WIDTH: f32 = 2.0; pub const ROAD_Z_OFFSET: f32 = 0.3; +pub const MAX_SLOPE: f32 = 0.25; // 25% grade diff --git a/simulation/src/map/objects/road.rs b/simulation/src/map/objects/road.rs index 0a93f18d..6a565db9 100644 --- a/simulation/src/map/objects/road.rs +++ b/simulation/src/map/objects/road.rs @@ -1,13 +1,13 @@ use serde::{Deserialize, Serialize}; use slotmapd::new_key_type; -use geom::{lerp, BoldLine, Degrees, PolyLine3}; -use geom::{PolyLine, Spline3}; +use geom::PolyLine; +use geom::{lerp, BoldLine, Degrees, PolyLine3, Spline, Spline1}; use geom::{Vec2, Vec3}; use crate::map::{ Environment, Intersection, IntersectionID, Lane, LaneDirection, LaneID, LaneKind, LanePattern, - Lanes, ParkingSpots, Roads, SpatialMap, ROAD_Z_OFFSET, + Lanes, ParkingSpots, Roads, SpatialMap, MAX_SLOPE, ROAD_Z_OFFSET, }; new_key_type! { @@ -68,6 +68,7 @@ pub enum PointGenerateError { impl Road { /// Builds the road and its associated lanes + /// Must not fail or it make keeping invariants very complicated be cause of road splitting pub fn make( src: &Intersection, dst: &Intersection, @@ -78,16 +79,19 @@ impl Road { lanes: &mut Lanes, parking: &mut ParkingSpots, spatial: &mut SpatialMap, - ) -> Option { + ) -> RoadID { let width = lane_pattern.width(); - let points = Self::generate_points( + let points = match Self::generate_points( src.pos, dst.pos, segment, lane_pattern.lanes().any(|(a, _, _)| a.is_rail()), env, - ) - .ok()?; + ) { + Ok(v) => v, + Err((Some(v), _)) => v, + _ => PolyLine3::new(vec![src.pos, dst.pos]), + }; let id = roads.insert_with_key(|id| Self { id, @@ -120,7 +124,7 @@ impl Road { road.update_lanes(lanes, parking); spatial.insert(id, road.boldline()); - Some(road.id) + road.id } pub fn is_one_way(&self) -> bool { @@ -278,18 +282,18 @@ impl Road { points.cut(self.interface_from(self.src), self.interface_from(self.dst)); let cpoints = &mut self.interfaced_points; - let i_beg = cpoints.first().z; - let i_end = cpoints.last().z; + let z_beg = self.points.first().z; + let z_end = self.points.last().z; let start = cpoints.first().clone(); let end = cpoints.last().clone(); for v in cpoints.iter_mut_unchecked() { - let start_coeff = v.distance(start) / 5.0; - v.z = lerp(i_beg, v.z, start_coeff.clamp(0.0, 1.0)); + let start_coeff = v.distance(start) / 15.0; + v.z = lerp(z_beg, v.z, start_coeff.clamp(0.0, 1.0)); - let end_coeff = v.distance(end) / 5.0; - v.z = lerp(i_end, v.z, end_coeff.clamp(0.0, 1.0)); + let end_coeff = v.distance(end) / 15.0; + v.z = lerp(z_end, v.z, end_coeff.clamp(0.0, 1.0)); } cpoints.recalculate_length(); @@ -312,7 +316,7 @@ impl Road { end_height: f32, maxslope: f32, env: &Environment, - ) -> Result { + ) -> Result, PointGenerateError)> { // first calculate the contour let mut contour = Vec::with_capacity(p.length() as usize + 2); @@ -325,7 +329,9 @@ impl Road { ) .chain(std::iter::once(p.last())) { - let h = env.height(pos).ok_or(PointGenerateError::OutsideOfMap)?; + let h = env + .height(pos) + .ok_or((None, PointGenerateError::OutsideOfMap))?; contour.push(h); points.push(pos.z(h)); } @@ -347,13 +353,15 @@ impl Road { } let mut cur_height = contour.last().copied().unwrap(); - let mut i = airborn.len(); - for &h in contour.iter().rev() { - i -= 1; - let diff = cur_height - h; + { + let mut i = airborn.len(); + for &h in contour.iter().rev() { + i -= 1; + let diff = cur_height - h; - airborn[i] |= diff > maxslope; - cur_height -= diff.min(maxslope); + airborn[i] |= diff > maxslope; + cur_height -= diff.min(maxslope); + } } // Then find the interface points where points become airborn @@ -365,33 +373,54 @@ impl Road { let mut interface = Vec::with_capacity(airborn.len()); for i in 1..airborn.len() - 1 { if airborn[i] && !airborn[i - 1] { - interface.push(i - 1); + interface.push(i.saturating_sub(3)); } if airborn[i] && !airborn[i + 1] { - interface.push(i + 1); + interface.push((i + 3).min(airborn.len() - 1)); } } - // Then linear interpolate the points between the interface points using a nice cubic - - fn cubic(t: f32) -> f32 { - t * t * (3.0 - 2.0 * t) + // merge interface points that overlap + let mut i = 0; + while i + 3 < interface.len() { + if interface[i + 1] >= interface[i + 2] { + interface.remove(i + 2); + interface.remove(i + 1); + } else { + i += 2; + } } - for w in interface.windows(2) { + let mut slope_was_too_steep = false; + + // Then linear interpolate the points between the interface points that aren't on the ground using a nice spline + for w in interface.chunks(2) { let i1 = w[0]; let i2 = w[1]; let h1 = contour[i1]; let h2 = contour[i2]; - let di = (i2 - i1) as f32; - let dh = h2 - h1; + let derivative_1 = h1 - contour.get(i1.wrapping_sub(1)).unwrap_or(&h1); + let derivative_2 = contour.get(i2 + 1).unwrap_or(&h2) - h2; + + let d = (h2 - h1).abs(); + + let slope = d / (i2 - i1) as f32; + + if slope > maxslope { + slope_was_too_steep = true; + } + + let s = Spline1 { + from: h1, + to: h2, + from_derivative: derivative_1 * d, + to_derivative: derivative_2 * d, + }; - for i in i1..=i2 { - let coeff = (i - i1) as f32 / di; - let h = h1 + dh * cubic(coeff); - contour[i] = h; + for (j, h) in s.points(i2 - i1 + 1).enumerate() { + contour[i1 + j] = h; } } @@ -416,6 +445,10 @@ impl Road { let mut points = PolyLine3::new(points); points.simplify(Degrees(1.0).into(), 1.0, 100.0); + if slope_was_too_steep { + return Err((Some(points), PointGenerateError::TooSteep)); + } + Ok(points) } @@ -425,35 +458,35 @@ impl Road { segment: RoadSegmentKind, precise: bool, env: &Environment, - ) -> Result { + ) -> Result, PointGenerateError)> { let spline = match segment { RoadSegmentKind::Straight => { let p = PolyLine::new(vec![from.xy(), to.xy()]); - return Self::heightfinder(&p, from.z, to.z, 0.25, env); + return Self::heightfinder(&p, from.z, to.z, MAX_SLOPE, env); } - RoadSegmentKind::Curved((from_derivative, to_derivative)) => Spline3 { - from: from.up(ROAD_Z_OFFSET), - to: to.up(ROAD_Z_OFFSET), - from_derivative: from_derivative.z0(), - to_derivative: to_derivative.z0(), + RoadSegmentKind::Curved((from_derivative, to_derivative)) => Spline { + from: from.xy(), + to: to.xy(), + from_derivative, + to_derivative, }, }; let iter = spline.smart_points(if precise { 0.1 } else { 1.0 }, 0.0, 1.0); - let mut p = PolyLine3::new(vec![from.up(ROAD_Z_OFFSET)]); + let mut p = PolyLine::new(vec![from.xy()]); for v in iter { - if v.is_close(from, 1.0) { + if v.is_close(from.xy(), 1.0) { continue; } - if v.is_close(to, 1.0) { + if v.is_close(to.xy(), 1.0) { continue; } p.push(v); } - p.push(to.up(ROAD_Z_OFFSET)); + p.push(to.xy()); - Ok(p) + Self::heightfinder(&p, from.z, to.z, MAX_SLOPE, env) } pub fn interface_point(&self, id: IntersectionID) -> Vec3 {