From 600fd0c27697b68804ef237585cb5ddf38225f45 Mon Sep 17 00:00:00 2001 From: Patrick Marsee Date: Fri, 3 Jan 2025 17:39:08 -0500 Subject: [PATCH] Fleshed out samplers, segments, and the rail graph. --- src/baked_segment.rs | 13 + src/lib.rs | 9 + src/rail_connector.rs | 14 + src/rail_graph.rs | 275 ++++++++++++++++++ src/rail_segment.rs | 136 +++++++++ src/samplers/circular_sampler.rs | 461 ++++++++++++++++++++++++++++++ src/samplers/mod.rs | 3 + src/samplers/sampler.rs | 78 +++++ src/samplers/spline_sampler_3d.rs | 34 +++ src/samplers/tangent_sampler.rs | 368 ++++++++++++++++++++++++ src/segment_endpoint.rs | 13 + src/simple_segment.rs | 379 ++++++++++++++++++++++++ 12 files changed, 1783 insertions(+) create mode 100644 src/baked_segment.rs create mode 100644 src/rail_connector.rs create mode 100644 src/rail_segment.rs create mode 100644 src/samplers/circular_sampler.rs create mode 100644 src/samplers/mod.rs create mode 100644 src/samplers/sampler.rs create mode 100644 src/samplers/spline_sampler_3d.rs create mode 100644 src/samplers/tangent_sampler.rs create mode 100644 src/segment_endpoint.rs create mode 100644 src/simple_segment.rs diff --git a/src/baked_segment.rs b/src/baked_segment.rs new file mode 100644 index 0000000..bb18b83 --- /dev/null +++ b/src/baked_segment.rs @@ -0,0 +1,13 @@ +use crate::prelude::*; +use std::string::String; +use std::vec::Vec; + +pub struct BakedSegment { + // Connections + lower_segment: String, + upper_segment: String, + + // Shape + bake_interval: f32, + shape: Vec, +} diff --git a/src/lib.rs b/src/lib.rs index 3217c7c..b585191 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,4 +1,9 @@ mod rail_graph; +mod rail_segment; +mod rail_connector; +mod samplers; +mod segment_endpoint; +mod simple_segment; mod prelude { pub use godot::prelude::*; @@ -10,3 +15,7 @@ struct RailNetworkExtension; #[gdextension] unsafe impl ExtensionLibrary for RailNetworkExtension {} + +//mod tests { +// use super::samplers::tests; +//} diff --git a/src/rail_connector.rs b/src/rail_connector.rs new file mode 100644 index 0000000..09e76ea --- /dev/null +++ b/src/rail_connector.rs @@ -0,0 +1,14 @@ +#[derive(Copy, Clone)] +pub struct RailConnector { + pub segment_index: usize, + pub endpoint_index: usize, +} + +impl RailConnector { + pub fn new(segment_index: usize, endpoint_index: usize) -> Self { + Self { + segment_index, + endpoint_index, + } + } +} diff --git a/src/rail_graph.rs b/src/rail_graph.rs index 3616db2..e690cb5 100644 --- a/src/rail_graph.rs +++ b/src/rail_graph.rs @@ -1 +1,276 @@ use crate::prelude::*; +use crate::rail_segment::RailSegment; +use crate::rail_connector::RailConnector; +//use std::collections::HashMap; +use std::vec::Vec; +//use std::string::String; + +pub struct RailGraph { + segments: Vec>>, + // segment_names: HashMap, + + min_available_index: usize, +} + +impl RailGraph { + pub fn new() -> Self { + Self { + segments: Vec::>>::new(), + //segment_names: {}, + min_available_index: 0, + } + } + + pub fn get_segment(&self, index: usize) -> Option<&Box> { + match &self.segments.get(index) { + Some(ret) => ret.as_ref(), + None => None, + } + } + + pub fn add_segment(&mut self, segment: Box) -> usize { + let ret = self.min_available_index; + if self.segments.len() == self.min_available_index { + self.segments.push(Some(segment)); + self.min_available_index = self.segments.len(); + } else { + self.segments[self.min_available_index] = Some(segment); + self.update_min_available_index(); + } + ret + } + + pub fn remove_segment(&mut self, index: usize) { + self.isolate_segment(index); + self.segments[index] = Option::None; + if index < self.min_available_index { + self.min_available_index = index; + } + } + + pub fn connect_endpoints(&mut self, index1: usize, endpoint1: usize, index2: usize, endpoint2: usize) { + // Needs improvement: silently fails with invalid endpoints. + let connector1 = RailConnector::new(index2, endpoint2); + let connector2 = RailConnector::new(index1, endpoint1); + // Before we continue, make sure that anything that these endpoints are already connected to are disconnected. + if let Some(segment1) = &self.segments[index1] { + if let Ok(old_connection) = segment1.borrow_connection(endpoint1) { + let old_index = old_connection.segment_index; + let old_endpoint = old_connection.endpoint_index; + if let Some(old_segment) = &mut self.segments[old_index] { + let _ = old_segment.remove_connection(old_endpoint); + } + } + } + if let Some(segment2) = &self.segments[index2] { + if let Ok(old_connection) = segment2.borrow_connection(endpoint2) { + let old_index = old_connection.segment_index; + let old_endpoint = old_connection.endpoint_index; + if let Some(old_segment) = &mut self.segments[old_index] { + let _ = old_segment.remove_connection(old_endpoint); + } + } + } + // Connect them now + if let Some(segment1) = &mut self.segments[index1] { + let _ = segment1.add_connection(endpoint1, connector1); + } + if let Some(segment2) = &mut self.segments[index2] { + let _ = segment2.add_connection(endpoint2, connector2); + } + } + + pub fn disconnect_endpoint(&mut self, index: usize, endpoint: usize) { + if let Some(segment) = &self.segments[index] { + if let Ok(old_connection) = segment.borrow_connection(endpoint) { + let old_index = old_connection.segment_index; + let old_endpoint = old_connection.endpoint_index; + if let Some(old_segment) = &mut self.segments[old_index] { + let _ = old_segment.remove_connection(old_endpoint); + } + } + } + if let Some(segment) = &mut self.segments[index] { + let _ = segment.remove_connection(endpoint); + } + } + + pub fn append_segment(&mut self, + new_segment: Box, + new_segment_endpoint: usize, + onto_segment: usize, + ont_segment_endpoint: usize) { + let index = self.add_segment(new_segment); + self.connect_endpoints(index, new_segment_endpoint, onto_segment, ont_segment_endpoint); + } + + fn isolate_segment(&mut self, index: usize) { + // Get the old connections + let mut connections = Vec::::new(); + if let Some(segment) = &self.segments[index] { + for connection in segment.connection_iter() { + connections.push(*connection); + } + } + + // disconnect neighbors + for connection in connections { + if let Some(old_segment) = &mut self.segments[connection.segment_index] { + let _ = old_segment.remove_connection(connection.endpoint_index); + } + } + + // reflect the same change in the original segment. + if let Some(segment) = &mut self.segments[index] { + segment.remove_all_connections(); + } + } + + fn update_min_available_index(&mut self) { + while let Some(_segment) = &self.segments[self.min_available_index] { + self.min_available_index += 1; + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::samplers::tangent_sampler::*; + use crate::samplers::circular_sampler::*; + use crate::samplers::sampler::*; + use crate::simple_segment::*; + use std::f32::consts::PI; + + /*fn compare_vectors(vec1: Vector3, vec2: Vector3, epsilon: f32, message: &str) { + assert!((vec1.x - vec2.x).abs() <= epsilon && (vec1.y - vec2.y).abs() <= epsilon, + "{message} Expected {vec2}, received {vec1}.") + }*/ + + /*fn compare_floats(float1: f32, float2: f32, epsilon: f32, message: &str) { + assert!((float1 - float2).abs() <= epsilon, + "{message} Expected {float2}, received {float1}.") + }*/ + + fn create_test_graph() -> RailGraph { + let alignment1 = TangentAlignmentSampler::new(Vector2::new(0.0, 0.0), Vector2::new(1.0, 0.0)); + let elevation1 = TangentElevationSampler::new(0.0, 0.0, 1.0); + let test_segment1 = Box::new(SimpleSegment::new(alignment1, elevation1)); + let alignment2 = CircularAlignmentSampler::new(0.0, PI * 0.5, Vector2::new(1.0, 1.0), 1.0, true); + let elevation2 = TangentElevationSampler::new(0.0, 0.0, alignment2.length()); + let test_segment2 = Box::new(SimpleSegment::new(alignment2, elevation2)); + let alignment3 = TangentAlignmentSampler::new(Vector2::new(2.0, 1.0), Vector2::new(2.0, 2.0)); + let elevation3 = TangentElevationSampler::new(0.0, 0.0, 1.0); + let test_segment3 = Box::new(SimpleSegment::new(alignment3, elevation3)); + + let mut test_graph = RailGraph::new(); + test_graph.add_segment(test_segment1); + test_graph.append_segment(test_segment2, 0, 0, 1); + test_graph.append_segment(test_segment3, 0, 1, 1); + return test_graph; + } + + #[test] + fn test_get_segment() { + let test_graph = create_test_graph(); + assert!(match test_graph.get_segment(0) { Option::Some(_)=>true, _=>false }, + "Segment 0 could not be retrieved."); + assert!(match test_graph.get_segment(1) { Option::Some(_)=>true, _=>false }, + "Segment 1 could not be retrieved."); + assert!(match test_graph.get_segment(2) { Option::Some(_)=>true, _=>false }, + "Segment 2 could not be retrieved."); + assert!(match test_graph.get_segment(3) { Option::None=>true, _=>false }, + "Segment that didn't exist was retrieved."); + } + + #[test] + fn test_add_segment() { + let alignment = TangentAlignmentSampler::new(Vector2::new(2.0, 2.0), Vector2::new(2.0, 3.0)); + let elevation = TangentElevationSampler::new(0.0, 0.0, 1.0); + let test_segment = Box::new(SimpleSegment::new(alignment, elevation)); + + let mut test_graph = create_test_graph(); + let index = test_graph.add_segment(test_segment); + assert_eq!(index, 3, "Segment was added with incorrect index."); + assert_eq!(test_graph.segments.len(), 4, "Graph has incorrect number of segments."); + } + + #[test] + fn test_remove_segment() { + let mut test_graph = create_test_graph(); + test_graph.remove_segment(1); + assert_eq!(test_graph.segments.len(), 3, "Graph has incorrect number of segments."); + assert_eq!(test_graph.min_available_index, 1, "Incorrect min_available_index."); + let first_segment = test_graph.get_segment(0).unwrap(); + assert!(!first_segment.check_connection(1).unwrap(), "Connection 1 wasn't broken."); + let second_segment = test_graph.get_segment(2).unwrap(); + assert!(!second_segment.check_connection(0).unwrap(), "Connection 2 wasn't broken."); + assert!(match test_graph.get_segment(1) { Option::None=>true, _=>false }, + "Segment that didn't exist was retrieved."); + } + + #[test] + fn test_connect_endpoints() { + let mut test_graph = create_test_graph(); + test_graph.connect_endpoints(0, 0, 2, 1); + let first_segment = test_graph.get_segment(0).unwrap(); + let first_connection = first_segment.borrow_connection(0).unwrap(); + assert_eq!(first_connection.segment_index, 2, "Connection(0, 0) had the incorrect index."); + assert_eq!(first_connection.endpoint_index, 1, "Connection(0, 0) had the incorrect endpoint."); + let second_segment = test_graph.get_segment(2).unwrap(); + let second_connection = second_segment.borrow_connection(1).unwrap(); + assert_eq!(second_connection.segment_index, 0, "Connection(2, 1) had the incorrect index."); + assert_eq!(second_connection.endpoint_index, 0, "Connection(2, 1) had the incorrect endpoint."); + test_graph.connect_endpoints(0, 1, 2, 0); + let first_segment = test_graph.get_segment(0).unwrap(); + let first_connection = first_segment.borrow_connection(1).unwrap(); + assert_eq!(first_connection.segment_index, 2, "Connection(0, 1) had the incorrect index."); + assert_eq!(first_connection.endpoint_index, 0, "Connection(0, 1) had the incorrect endpoint."); + let second_segment = test_graph.get_segment(2).unwrap(); + let second_connection = second_segment.borrow_connection(0).unwrap(); + assert_eq!(second_connection.segment_index, 0, "Connection(2, 0) had the incorrect index."); + assert_eq!(second_connection.endpoint_index, 1, "Connection(2, 0) had the incorrect endpoint."); + let middle_segment = test_graph.get_segment(1).unwrap(); + let first_connection = middle_segment.borrow_connection(0); + assert!(match first_connection { Result::Err(_)=>true, _=>false }, + "Connection(1, 0) still exists."); + let second_connection = middle_segment.borrow_connection(1); + assert!(match second_connection { Result::Err(_)=>true, _=>false }, + "Connection(1, 1) still exists.") + } + + #[test] + fn test_disconnect_endpoint() { + let mut test_graph = create_test_graph(); + test_graph.disconnect_endpoint(0, 1); + let first_segment = test_graph.get_segment(0).unwrap(); + let first_connection = first_segment.borrow_connection(1); + assert!(match first_connection { Result::Err(_)=>true, _=>false }, + "Connection(0, 1) still exists."); + let middle_segment = test_graph.get_segment(1).unwrap(); + let middle_connection = middle_segment.borrow_connection(0); + assert!(match middle_connection { Result::Err(_)=>true, _=>false }, + "Connection(1, 0) still exists."); + } + + #[test] + fn test_append_segment() { + let test_graph = create_test_graph(); + let first_segment = test_graph.get_segment(0).unwrap(); + let second_segment = test_graph.get_segment(1).unwrap(); + let third_segment = test_graph.get_segment(2).unwrap(); + + let connection01 = first_segment.borrow_connection(1).unwrap(); + assert_eq!(connection01.segment_index, 1, "Connection(0, 1) had the incorrect index."); + assert_eq!(connection01.endpoint_index, 0, "Connection(0, 1) had the incorrect endpoint."); + let connection10 = second_segment.borrow_connection(0).unwrap(); + assert_eq!(connection10.segment_index, 0, "Connection(1, 0) had the incorrect index."); + assert_eq!(connection10.endpoint_index, 1, "Connection(1, 0) had the incorrect endpoint."); + let connection11 = second_segment.borrow_connection(1).unwrap(); + assert_eq!(connection11.segment_index, 2, "Connection(1, 1) had the incorrect index."); + assert_eq!(connection11.endpoint_index, 0, "Connection(1, 1) had the incorrect endpoint."); + let connection20 = third_segment.borrow_connection(0).unwrap(); + assert_eq!(connection20.segment_index, 1, "Connection(2, 0) had the incorrect index."); + assert_eq!(connection20.endpoint_index, 1, "Connection(2, 0) had the incorrect endpoint."); + } +} diff --git a/src/rail_segment.rs b/src/rail_segment.rs new file mode 100644 index 0000000..e083bc8 --- /dev/null +++ b/src/rail_segment.rs @@ -0,0 +1,136 @@ +use crate::prelude::*; +use crate::rail_connector::RailConnector; +use std::error; +use std::fmt; +use std::iter::Iterator; + +#[derive(Debug)] +pub enum Error { + IndexError, + EndpointError, + OffsetError, + NeighborError, +} + +impl fmt::Display for Error { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Self::IndexError => write!(f, "Nonexistent internal curve index"), + Self::EndpointError => write!(f, "Nonexistent endpoint index"), + Self::OffsetError => write!(f, "Invalid offset"), + Self::NeighborError => write!(f, "Invalid Neighbor"), + } + } +} + +impl error::Error for Error {} + +pub struct ConnectionIter<'a> { + segment: &'a dyn RailSegment, + index: usize, +} + +impl<'a> ConnectionIter<'a> { + pub fn new(segment: &'a impl RailSegment) -> Self { + Self { + segment, + index: 0, + } + } +} + +impl<'a> Iterator for ConnectionIter<'a> { + type Item = &'a RailConnector; + + fn next(&mut self) -> Option { + while self.index < self.segment.num_endpoints() { + if let Ok(connection) = self.segment.borrow_connection(self.index) { + self.index += 1; + return Some(connection); + } else { + self.index += 1; + } + } + return None; + } +} + +/** A RailSegment represents a section of track. + * They contain at least one internal curve which is addressibe via an integer + * index. Valid indecies start at 0, though a negative index may be used to + * indicate that a point does not exist on any internal curve. Offsets are in + * Godot units (which shall be interpreted as meters). + */ +pub trait RailSegment { + + /** Get the overall length of the internal curve represented by the given + * `index`. + */ + fn get_length(&self, index: usize) -> Result; + + /** Get the location defined by the given `offset` on the internal curve + * represented by the given `index`. + */ + fn sample(&self, index: usize, offset: f32) -> Result; + + /** Get the location defined by the given `offset` on the internal curve + * represented by the given `index`, but with additional rotation data. + */ + fn sample_with_rotation(&self, index: usize, offset: f32) -> Result; + + /** Similar to sample_with_rotation, but the rotation is facing the opposite direction. + */ + fn sample_with_rotation_reverse(&self, index: usize, offset: f32) -> Result; + + /** Get the direction that constitutes "up" for the given `offset` on the + * internal curve represented by the given `index`. + */ + fn sample_up_vector(&self, index: usize, offset: f32) -> Result; + + // endpoint functions + + /** Get the segment index associated with an endpoint. */ + fn segment_from_endpoint(&self, endpoint: usize) -> Result; + + /** Get the location of an endpoint. */ + fn get_endpoint_transform(&self, endpoint: usize) -> Result; + + /** Get whether an endpoint represents the upper or lower end of a segment. */ + fn is_endpoint_upper(&self, endpoint: usize) -> Result; + + /** Get the number of endpoins this segment has. */ + fn num_endpoints(&self) -> usize; + + // connection functions + + /** Borrow the connector associated with the given endpoint. */ + fn borrow_connection(&self, endpoint: usize) -> Result<&RailConnector, Error>; + + /** Check if a connection is established on the given endpoint. */ + fn check_connection(&self, endpoint: usize) -> Result; + + /** Get an iterator of the connections on this segment. + * If an endpoint is not connected, then it will not be represented by this iterator. + * This iterator keeps an immutable reference to the segment from which it was created. + */ + fn connection_iter(&self) -> ConnectionIter; + + /** Add a connection to the given endpoint. + * Any previous connection on it is overwritten. + */ + fn add_connection(&mut self, endpoint: usize, connector: RailConnector) -> Result; + + /** Remove the given endpoint's connection if it has one. */ + fn remove_connection(&mut self, endpoint: usize) -> Result; + + /** Remove all connections from the segment. */ + fn remove_all_connections(&mut self); + + // internal variable state functions + + /** Get the internal state float value. */ + fn get_state(&self) -> f32; + + /** Set the internal state float value. */ + fn set_state(&mut self, value: f32); +} diff --git a/src/samplers/circular_sampler.rs b/src/samplers/circular_sampler.rs new file mode 100644 index 0000000..5386508 --- /dev/null +++ b/src/samplers/circular_sampler.rs @@ -0,0 +1,461 @@ +use crate::prelude::*; +use crate::samplers::sampler::*; +use std::f32::consts::PI; + +/** + * Samples a segment of a circle. + * Angles are as on a compass, starting with 0 at North and increasing clockwise. + * North is Vector2(0.0, -1.0), East is Vector2(1.0, 0.0) + */ +pub struct CircularAlignmentSampler { + lower_end_rad: f32, + upper_end_rad: f32, + center: Vector2, + radius_meters: f32, + clockwise: bool, +} + +impl CircularAlignmentSampler { + pub fn new(mut lower_end_rad: f32, mut upper_end_rad: f32, center: Vector2, radius_meters: f32, clockwise: bool) -> Self { + if clockwise && upper_end_rad < lower_end_rad { + upper_end_rad += 2.0 * PI; + } else if !clockwise && upper_end_rad > lower_end_rad { + lower_end_rad += 2.0 * PI; + } + Self { + lower_end_rad, + upper_end_rad, + center, + radius_meters, + clockwise, + } + } + + fn angle_from_normalized(&self, t: f32) -> f32 { + self.lower_end_rad * (1.0 - t) + self.upper_end_rad * t + } +} + +impl AlignmentSampler for CircularAlignmentSampler { + fn sample_normalized(&self, t: f32) -> Vector2 { + let t_rad = self.angle_from_normalized(t); + return Vector2::new(t_rad.sin() * self.radius_meters, -t_rad.cos() * self.radius_meters) + self.center; + } + + fn tangent_normalized(&self, t: f32) -> Vector2 { + let t_rad = self.angle_from_normalized(t); + if self.clockwise { + return Vector2::new(t_rad.cos(), t_rad.sin()); + } else { + return Vector2::new(-t_rad.cos(), -t_rad.sin()); + } + } + + fn sample_offset(&self, offset: f32) -> Vector2 { + self.sample_normalized(offset / self.length()) + } + + fn tangent_offset(&self, offset: f32) -> Vector2 { + self.tangent_normalized(offset / self.length()) + } + + fn length(&self) -> f32 { + (self.upper_end_rad - self.lower_end_rad).abs() * self.radius_meters + } +} + +pub struct CircularElevationSampler { + lower_end: f32, + upper_end: f32, + center_meters: f32, + radius_meters: f32, + top_down_length: f32, + peak: bool, // as opposed to valley +} + +impl CircularElevationSampler { + pub fn new(lower_end: f32, + upper_end: f32, + center_meters: f32, + radius_meters: f32, + peak: bool,) -> Self { + let top_down_length = (upper_end - lower_end) * radius_meters; + Self { + lower_end, + upper_end, + center_meters, + radius_meters, + top_down_length, + peak, + } + } +} + +fn circle_function(x: f32) -> f32 { + let x = x.clamp(-1.0, 1.0); + (1.0 - x * x).sqrt() +} + +impl ElevationSampler for CircularElevationSampler { + fn sample_normalized(&self, top_down_t: f32) -> f32 { + let actual_t = self.lower_end * (1.0 - top_down_t) + self.upper_end * top_down_t; + if self.peak { + return circle_function(actual_t) * self.radius_meters + self.center_meters; + } else { + return -circle_function(actual_t) * self.radius_meters + self.center_meters; + } + } + + fn grade_normalized(&self, top_down_t: f32) -> f32 { + let actual_t = self.lower_end * (1.0 - top_down_t) + self.upper_end * top_down_t; + if self.peak { + return -actual_t / circle_function(actual_t); + } else { + return actual_t / circle_function(actual_t); + } + } + + fn sample_offset(&self, offset: f32) -> f32 { + self.sample_normalized(offset / self.top_down_length) + } + + fn grade_offset(&self, offset: f32) -> f32 { + self.grade_normalized(offset / self.top_down_length) + } + + fn length(&self) -> f32 { + (self.upper_end.asin() - self.lower_end.asin()) * self.radius_meters + } + + fn to_top_down_normalized(&self, actual_t: f32) -> f32 { + let actual_t = self.lower_end.asin() * (1.0 - actual_t) + self.upper_end.asin() * actual_t; + (actual_t.sin() - self.lower_end) / (self.upper_end - self.lower_end) + } + + fn to_top_down_offset(&self, actual_offset: f32) -> f32 { + self.to_top_down_normalized(actual_offset / self.length()) * self.top_down_length + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn compare_vectors(vec1: Vector2, vec2: Vector2, message: &str) { + let epsilon = 1e-5; // This is really bad tolerance, but f32 sucks so I have no choice. + assert!((vec1.x - vec2.x).abs() <= epsilon && (vec1.y - vec2.y).abs() <= epsilon, + "{message} Expected {vec2}, received {vec1}.") + } + + fn compare_floats(float1: f32, float2: f32, message: &str) { + let epsilon = 1e-5; + assert!((float1 - float2).abs() <= epsilon, + "{message} Expected {float2}, received {float1}.") + } + + #[test] + fn test_circular_alignment_sample_normalized() { + let test_alignment_1 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, true); + compare_vectors(test_alignment_1.sample_normalized(0.0), Vector2::new(1.0, -1.0), + "test_alignment_1 begins in the wrong location."); + compare_vectors(test_alignment_1.sample_normalized(1.0), Vector2::new(1.0, 3.0), + "test_alignment_1 ends in the wrong location."); + compare_vectors(test_alignment_1.sample_normalized(0.5), Vector2::new(3.0, 1.0), + "test_alignment_1 passes through the wrong location."); + compare_vectors(test_alignment_1.sample_normalized(-1.0), Vector2::new(1.0, 3.0), + "test_alignment_1 negative offset ends in the wrong location."); + compare_vectors(test_alignment_1.sample_normalized(2.0), Vector2::new(1.0, -1.0), + "test_alignment_1 double offset ends in the wrong location."); + let test_alignment_2 = CircularAlignmentSampler::new(3.0 * PI / 2.0, PI / 2.0, Vector2::new(1.0, 1.0), 2.0, true); + compare_vectors(test_alignment_2.sample_normalized(0.0), Vector2::new(-1.0, 1.0), + "test_alignment_2 begins in the wrong location."); + compare_vectors(test_alignment_2.sample_normalized(1.0), Vector2::new(3.0, 1.0), + "test_alignment_2 ends in the wrong location."); + compare_vectors(test_alignment_2.sample_normalized(0.5), Vector2::new(1.0, -1.0), + "test_alignment_2 passes through the wrong location."); + compare_vectors(test_alignment_2.sample_normalized(-1.0), Vector2::new(3.0, 1.0), + "test_alignment_2 negative offset ends in the wrong location."); + compare_vectors(test_alignment_2.sample_normalized(2.0), Vector2::new(-1.0, 1.0), + "test_alignment_2 double offset ends in the wrong location."); + let test_alignment_3 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, false); + compare_vectors(test_alignment_3.sample_normalized(0.0), Vector2::new(1.0, -1.0), + "test_alignment_3 begins in the wrong location."); + compare_vectors(test_alignment_3.sample_normalized(1.0), Vector2::new(1.0, 3.0), + "test_alignment_3 ends in the wrong location."); + compare_vectors(test_alignment_3.sample_normalized(0.5), Vector2::new(-1.0, 1.0), + "test_alignment_3 passes through the wrong location."); + compare_vectors(test_alignment_3.sample_normalized(-1.0), Vector2::new(1.0, 3.0), + "test_alignment_3 negative offset ends in the wrong location."); + compare_vectors(test_alignment_3.sample_normalized(2.0), Vector2::new(1.0, -1.0), + "test_alignment_3 double offset ends in the wrong location."); + let test_alignment_4 = CircularAlignmentSampler::new(3.0 * PI / 2.0, PI / 2.0, Vector2::new(1.0, 1.0), 2.0, false); + compare_vectors(test_alignment_4.sample_normalized(0.0), Vector2::new(-1.0, 1.0), + "test_alignment_4 begins in the wrong location."); + compare_vectors(test_alignment_4.sample_normalized(1.0), Vector2::new(3.0, 1.0), + "test_alignment_4 ends in the wrong location."); + compare_vectors(test_alignment_4.sample_normalized(0.5), Vector2::new(1.0, 3.0), + "test_alignment_4 passes through the wrong location."); + compare_vectors(test_alignment_4.sample_normalized(-1.0), Vector2::new(3.0, 1.0), + "test_alignment_4 negative offset ends in the wrong location."); + compare_vectors(test_alignment_4.sample_normalized(2.0), Vector2::new(-1.0, 1.0), + "test_alignment_4 double offset ends in the wrong location."); + } + + #[test] + fn test_circular_alignment_tangent_normalized() { + let test_alignment_1 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, true); + compare_vectors(test_alignment_1.tangent_normalized(0.0), Vector2::new(1.0, 0.0), + "test_alignment_1 begins in the wrong direction."); + compare_vectors(test_alignment_1.tangent_normalized(1.0), Vector2::new(-1.0, 0.0), + "test_alignment_1 ends in the wrong direction."); + compare_vectors(test_alignment_1.tangent_normalized(0.5), Vector2::new(0.0, 1.0), + "test_alignment_1 passes through the wrong direction."); + compare_vectors(test_alignment_1.tangent_normalized(-1.0), Vector2::new(-1.0, 0.0), + "test_alignment_1 negative offset ends in the wrong direction."); + compare_vectors(test_alignment_1.tangent_normalized(2.0), Vector2::new(1.0, 0.0), + "test_alignment_1 double offset ends in the wrong direction."); + let test_alignment_2 = CircularAlignmentSampler::new(3.0 * PI / 2.0, PI / 2.0, Vector2::new(1.0, 1.0), 2.0, true); + compare_vectors(test_alignment_2.tangent_normalized(0.0), Vector2::new(0.0, -1.0), + "test_alignment_2 begins in the wrong direction."); + compare_vectors(test_alignment_2.tangent_normalized(1.0), Vector2::new(0.0, 1.0), + "test_alignment_2 ends in the wrong direction."); + compare_vectors(test_alignment_2.tangent_normalized(0.5), Vector2::new(1.0, 0.0), + "test_alignment_2 passes through the wrong direction."); + compare_vectors(test_alignment_2.tangent_normalized(-1.0), Vector2::new(0.0, 1.0), + "test_alignment_2 negative offset ends in the wrong direction."); + compare_vectors(test_alignment_2.tangent_normalized(2.0), Vector2::new(0.0, -1.0), + "test_alignment_2 double offset ends in the wrong direction."); + let test_alignment_3 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, false); + compare_vectors(test_alignment_3.tangent_normalized(0.0), Vector2::new(-1.0, 0.0), + "test_alignment_3 begins in the wrong direction."); + compare_vectors(test_alignment_3.tangent_normalized(1.0), Vector2::new(1.0, 0.0), + "test_alignment_3 ends in the wrong direction."); + compare_vectors(test_alignment_3.tangent_normalized(0.5), Vector2::new(0.0, 1.0), + "test_alignment_3 passes through the wrong direction."); + compare_vectors(test_alignment_3.tangent_normalized(-1.0), Vector2::new(1.0, 0.), + "test_alignment_3 negative offset ends in the wrong direction."); + compare_vectors(test_alignment_3.tangent_normalized(2.0), Vector2::new(-1.0, 0.0), + "test_alignment_3 double offset ends in the wrong direction."); + let test_alignment_4 = CircularAlignmentSampler::new(3.0 * PI / 2.0, PI / 2.0, Vector2::new(1.0, 1.0), 2.0, false); + compare_vectors(test_alignment_4.tangent_normalized(0.0), Vector2::new(0.0, 1.0), + "test_alignment_4 begins in the wrong direction."); + compare_vectors(test_alignment_4.tangent_normalized(1.0), Vector2::new(0.0, -1.0), + "test_alignment_4 ends in the wrong direction."); + compare_vectors(test_alignment_4.tangent_normalized(0.5), Vector2::new(1.0, 0.0), + "test_alignment_4 passes through the wrong direction."); + compare_vectors(test_alignment_4.tangent_normalized(-1.0), Vector2::new(0.0, -1.0), + "test_alignment_4 negative offset ends in the wrong direction."); + compare_vectors(test_alignment_4.tangent_normalized(2.0), Vector2::new(0.0, 1.0), + "test_alignment_4 double offset ends in the wrong direction."); + } + + #[test] + fn test_circular_alignment_sample_offset() { + let test_alignment_1 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, true); + compare_vectors(test_alignment_1.sample_offset(0.0), Vector2::new(1.0, -1.0), + "test_alignment_1 begins in the wrong location."); + compare_vectors(test_alignment_1.sample_offset(2.0 * PI), Vector2::new(1.0, 3.0), + "test_alignment_1 ends in the wrong location."); + compare_vectors(test_alignment_1.sample_offset(PI), Vector2::new(3.0, 1.0), + "test_alignment_1 passes through the wrong location."); + compare_vectors(test_alignment_1.sample_offset(-2.0 * PI), Vector2::new(1.0, 3.0), + "test_alignment_1 negative offset ends in the wrong location."); + compare_vectors(test_alignment_1.sample_offset(4.0 * PI), Vector2::new(1.0, -1.0), + "test_alignment_1 double offset ends in the wrong location."); + let test_alignment_2 = CircularAlignmentSampler::new(3.0 * PI / 2.0, PI / 2.0, Vector2::new(1.0, 1.0), 2.0, true); + compare_vectors(test_alignment_2.sample_offset(0.0), Vector2::new(-1.0, 1.0), + "test_alignment_2 begins in the wrong location."); + compare_vectors(test_alignment_2.sample_offset(2.0 * PI), Vector2::new(3.0, 1.0), + "test_alignment_2 ends in the wrong location."); + compare_vectors(test_alignment_2.sample_offset(PI), Vector2::new(1.0, -1.0), + "test_alignment_2 passes through the wrong location."); + compare_vectors(test_alignment_2.sample_offset(-2.0 * PI), Vector2::new(3.0, 1.0), + "test_alignment_2 negative offset ends in the wrong location."); + compare_vectors(test_alignment_2.sample_offset(4.0 * PI), Vector2::new(-1.0, 1.0), + "test_alignment_2 double offset ends in the wrong location."); + let test_alignment_3 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, false); + compare_vectors(test_alignment_3.sample_offset(0.0), Vector2::new(1.0, -1.0), + "test_alignment_3 begins in the wrong location."); + compare_vectors(test_alignment_3.sample_offset(2.0 * PI), Vector2::new(1.0, 3.0), + "test_alignment_3 ends in the wrong location."); + compare_vectors(test_alignment_3.sample_offset(PI), Vector2::new(-1.0, 1.0), + "test_alignment_3 passes through the wrong location."); + compare_vectors(test_alignment_3.sample_offset(-2.0 * PI), Vector2::new(1.0, 3.0), + "test_alignment_3 negative offset ends in the wrong location."); + compare_vectors(test_alignment_3.sample_offset(4.0 * PI), Vector2::new(1.0, -1.0), + "test_alignment_3 double offset ends in the wrong location."); + let test_alignment_4 = CircularAlignmentSampler::new(3.0 * PI / 2.0, PI / 2.0, Vector2::new(1.0, 1.0), 2.0, false); + compare_vectors(test_alignment_4.sample_offset(0.0), Vector2::new(-1.0, 1.0), + "test_alignment_4 begins in the wrong location."); + compare_vectors(test_alignment_4.sample_offset(2.0 * PI), Vector2::new(3.0, 1.0), + "test_alignment_4 ends in the wrong location."); + compare_vectors(test_alignment_4.sample_offset(PI), Vector2::new(1.0, 3.0), + "test_alignment_4 passes through the wrong location."); + compare_vectors(test_alignment_4.sample_offset(-2.0 * PI), Vector2::new(3.0, 1.0), + "test_alignment_4 negative offset ends in the wrong location."); + compare_vectors(test_alignment_4.sample_offset(4.0 * PI), Vector2::new(-1.0, 1.0), + "test_alignment_4 double offset ends in the wrong location."); + } + + #[test] + fn test_circular_alignment_tangent_offset() { + let test_alignment_1 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, true); + compare_vectors(test_alignment_1.tangent_offset(0.0), Vector2::new(1.0, 0.0), + "test_alignment_1 begins in the wrong direction."); + compare_vectors(test_alignment_1.tangent_offset(2.0 * PI), Vector2::new(-1.0, 0.0), + "test_alignment_1 ends in the wrong direction."); + compare_vectors(test_alignment_1.tangent_offset(PI), Vector2::new(0.0, 1.0), + "test_alignment_1 passes through the wrong direction."); + compare_vectors(test_alignment_1.tangent_offset(-2.0 * PI), Vector2::new(-1.0, 0.0), + "test_alignment_1 negative offset ends in the wrong direction."); + compare_vectors(test_alignment_1.tangent_offset(4.0 * PI), Vector2::new(1.0, 0.0), + "test_alignment_1 double offset ends in the wrong direction."); + let test_alignment_2 = CircularAlignmentSampler::new(3.0 * PI / 2.0, PI / 2.0, Vector2::new(1.0, 1.0), 2.0, true); + compare_vectors(test_alignment_2.tangent_offset(0.0), Vector2::new(0.0, -1.0), + "test_alignment_2 begins in the wrong direction."); + compare_vectors(test_alignment_2.tangent_offset(2.0 * PI), Vector2::new(0.0, 1.0), + "test_alignment_2 ends in the wrong direction."); + compare_vectors(test_alignment_2.tangent_offset(PI), Vector2::new(1.0, 0.0), + "test_alignment_2 passes through the wrong direction."); + compare_vectors(test_alignment_2.tangent_offset(-2.0 * PI), Vector2::new(0.0, 1.0), + "test_alignment_2 negative offset ends in the wrong direction."); + compare_vectors(test_alignment_2.tangent_offset(4.0 * PI), Vector2::new(0.0, -1.0), + "test_alignment_2 double offset ends in the wrong direction."); + let test_alignment_3 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, false); + compare_vectors(test_alignment_3.tangent_offset(0.0), Vector2::new(-1.0, 0.0), + "test_alignment_3 begins in the wrong direction."); + compare_vectors(test_alignment_3.tangent_offset(2.0 * PI), Vector2::new(1.0, 0.0), + "test_alignment_3 ends in the wrong direction."); + compare_vectors(test_alignment_3.tangent_offset(PI), Vector2::new(0.0, 1.0), + "test_alignment_3 passes through the wrong direction."); + compare_vectors(test_alignment_3.tangent_offset(-2.0 * PI), Vector2::new(1.0, 0.), + "test_alignment_3 negative offset ends in the wrong direction."); + compare_vectors(test_alignment_3.tangent_offset(4.0 * PI), Vector2::new(-1.0, 0.0), + "test_alignment_3 double offset ends in the wrong direction."); + let test_alignment_4 = CircularAlignmentSampler::new(3.0 * PI / 2.0, PI / 2.0, Vector2::new(1.0, 1.0), 2.0, false); + compare_vectors(test_alignment_4.tangent_offset(0.0), Vector2::new(0.0, 1.0), + "test_alignment_4 begins in the wrong direction."); + compare_vectors(test_alignment_4.tangent_offset(2.0 * PI), Vector2::new(0.0, -1.0), + "test_alignment_4 ends in the wrong direction."); + compare_vectors(test_alignment_4.tangent_offset(PI), Vector2::new(1.0, 0.0), + "test_alignment_4 passes through the wrong direction."); + compare_vectors(test_alignment_4.tangent_offset(-2.0 * PI), Vector2::new(0.0, -1.0), + "test_alignment_4 negative offset ends in the wrong direction."); + compare_vectors(test_alignment_4.tangent_offset(4.0 * PI), Vector2::new(0.0, 1.0), + "test_alignment_4 double offset ends in the wrong direction."); + } + + #[test] + fn test_circular_alignment_length() { + let test_alignment_1 = CircularAlignmentSampler::new(0.0, PI, Vector2::new(1.0, 1.0), 2.0, true); + compare_floats(test_alignment_1.length(), PI * 2.0, "test_alignment_1 had an incorrect length."); + } + + #[test] + fn test_circular_elevation_sample_normalized() { + let test_elevation_1 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, true); + compare_floats(test_elevation_1.sample_normalized(0.0), 1.0 + 3.0_f32.sqrt(), + "test_elevation_1 begins in the wrong location."); + compare_floats(test_elevation_1.sample_normalized(1.0), 1.0 + 3.0_f32.sqrt(), + "test_elevation_1 ends in the wrong location."); + compare_floats(test_elevation_1.sample_normalized(0.5), 3.0, + "test_elevation_1 passes through the wrong location."); + let test_elevation_2 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, false); + compare_floats(test_elevation_2.sample_normalized(0.0), 1.0 - 3.0_f32.sqrt(), + "test_elevation_2 begins in the wrong location."); + compare_floats(test_elevation_2.sample_normalized(1.0), 1.0 - 3.0_f32.sqrt(), + "test_elevation_2 ends in the wrong location."); + compare_floats(test_elevation_2.sample_normalized(0.5), -1.0, + "test_elevation_2 passes through the wrong location."); + } + + #[test] + fn test_circular_elevation_grade_normalized() { + let test_elevation_1 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, true); + compare_floats(test_elevation_1.grade_normalized(0.0), 1.0 / 3.0_f32.sqrt(), + "test_elevation_1 begins in the wrong grade."); + compare_floats(test_elevation_1.grade_normalized(1.0), -1.0 / 3.0_f32.sqrt(), + "test_elevation_1 ends in the wrong grade."); + compare_floats(test_elevation_1.grade_normalized(0.5), 0.0, + "test_elevation_1 passes through the wrong grade."); + let test_elevation_2 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, false); + compare_floats(test_elevation_2.grade_normalized(0.0), -1.0 / 3.0_f32.sqrt(), + "test_elevation_2 begins in the wrong grade."); + compare_floats(test_elevation_2.grade_normalized(1.0), 1.0 / 3.0_f32.sqrt(), + "test_elevation_2 ends in the wrong grade."); + compare_floats(test_elevation_2.grade_normalized(0.5), 0.0, + "test_elevation_2 passes through the wrong grade."); + } + + #[test] + fn test_circular_elevation_sample_offset() { + let test_elevation_1 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, true); + compare_floats(test_elevation_1.sample_offset(0.0), 1.0 + 3.0_f32.sqrt(), + "test_elevation_1 begins in the wrong location."); + compare_floats(test_elevation_1.sample_offset(2.0), 1.0 + 3.0_f32.sqrt(), + "test_elevation_1 ends in the wrong location."); + compare_floats(test_elevation_1.sample_offset(1.0), 3.0, + "test_elevation_1 passes through the wrong location."); + let test_elevation_2 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, false); + compare_floats(test_elevation_2.sample_offset(0.0), 1.0 - 3.0_f32.sqrt(), + "test_elevation_2 begins in the wrong location."); + compare_floats(test_elevation_2.sample_offset(2.0), 1.0 - 3.0_f32.sqrt(), + "test_elevation_2 ends in the wrong location."); + compare_floats(test_elevation_2.sample_offset(1.0), -1.0, + "test_elevation_2 passes through the wrong location."); + } + + #[test] + fn test_circular_elevation_grade_offset() { + let test_elevation_1 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, true); + compare_floats(test_elevation_1.grade_offset(0.0), 1.0 / 3.0_f32.sqrt(), + "test_elevation_1 begins in the wrong grade."); + compare_floats(test_elevation_1.grade_offset(2.0), -1.0 / 3.0_f32.sqrt(), + "test_elevation_1 ends in the wrong grade."); + compare_floats(test_elevation_1.grade_offset(1.0), 0.0, + "test_elevation_1 passes through the wrong grade."); + let test_elevation_2 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, false); + compare_floats(test_elevation_2.grade_offset(0.0), -1.0 / 3.0_f32.sqrt(), + "test_elevation_2 begins in the wrong grade."); + compare_floats(test_elevation_2.grade_offset(2.0), 1.0 / 3.0_f32.sqrt(), + "test_elevation_2 ends in the wrong grade."); + compare_floats(test_elevation_2.grade_offset(1.0), 0.0, + "test_elevation_2 passes through the wrong grade."); + } + + #[test] + fn test_circular_elevation_length() { + let test_elevation_1 = CircularElevationSampler::new(-0.5, 0.5, 1.0, 2.0, true); + compare_floats(test_elevation_1.length(), 2.0 * PI / 3.0, "test_elevation_1 had an incorrect length."); + } + + #[test] + fn test_circular_elevation_to_top_down_normalized() { + let test_elevation_1 = CircularElevationSampler::new(-1.0, 1.0, 1.0, 2.0, true); + compare_floats(test_elevation_1.to_top_down_normalized(0.0), 0.0, + "test_elevation_1 begins in the wrong location."); + compare_floats(test_elevation_1.to_top_down_normalized(1.0), 1.0, + "test_elevation_1 ends in the wrong location."); + compare_floats(test_elevation_1.to_top_down_normalized(0.5), 0.5, + "test_elevation_1 passes through the wrong location."); + compare_floats(test_elevation_1.to_top_down_normalized(0.25), (1.0 - (2.0_f32.sqrt() / 2.0)) / 2.0, + "test_elevation_1 quarter offset ends in the wrong location."); + compare_floats(test_elevation_1.to_top_down_normalized(0.75), ((2.0_f32.sqrt() / 2.0) + 1.0) / 2.0, + "test_elevation_1 three quarter offset ends in the wrong location."); + } + + #[test] + fn test_circular_elevation_to_top_down_offset() { + let test_elevation_1 = CircularElevationSampler::new(-1.0, 1.0, 1.0, 2.0, true); + compare_floats(test_elevation_1.to_top_down_offset(0.0), 0.0, + "test_elevation_1 begins in the wrong location."); + compare_floats(test_elevation_1.to_top_down_offset(2.0 * PI), 4.0, + "test_elevation_1 ends in the wrong location."); + compare_floats(test_elevation_1.to_top_down_offset(PI), 2.0, + "test_elevation_1 passes through the wrong location."); + compare_floats(test_elevation_1.to_top_down_offset(PI / 2.0), (1.0 - (2.0_f32.sqrt() / 2.0)) * 2.0, + "test_elevation_1 quarter offset ends in the wrong location."); + compare_floats(test_elevation_1.to_top_down_offset(3.0 * PI / 2.0), ((2.0_f32.sqrt() / 2.0) + 1.0) * 2.0, + "test_elevation_1 three quarter offset ends in the wrong location."); + } +} diff --git a/src/samplers/mod.rs b/src/samplers/mod.rs new file mode 100644 index 0000000..3a3bafe --- /dev/null +++ b/src/samplers/mod.rs @@ -0,0 +1,3 @@ +pub mod sampler; +pub mod tangent_sampler; +pub mod circular_sampler; diff --git a/src/samplers/sampler.rs b/src/samplers/sampler.rs new file mode 100644 index 0000000..88c57cc --- /dev/null +++ b/src/samplers/sampler.rs @@ -0,0 +1,78 @@ +use crate::prelude::*; + +/** + * Implement this for 2D top-down point samplers. + * These samplers will be used for a segment's top-down shape. + */ +pub trait AlignmentSampler { + + /** + * Input distance from lower end in percent, get the top-down coordinates. + */ + fn sample_normalized(&self, t: f32) -> Vector2; + + /** + * Input distance from lower end in percent, get the normalized direction of the curve at that point. + */ + fn tangent_normalized(&self, t: f32) -> Vector2; + + /** + * Input distance from lower end in meters, get the top-down coordinates. + */ + fn sample_offset(&self, offset: f32) -> Vector2; + + /** + * Input distance from lower end in meters, get the normalized direction of the curve at that point. + */ + fn tangent_offset(&self, offset: f32) -> Vector2; + + /** + * Get the length as seen from above. This does not account for elevation changes. + */ + fn length(&self) -> f32; +} + +/** + * Implement this for 1D elevation point samplers. + * These samplers will be used for a segment's elevation or superelevation. + * Structs that implement this should keep track of their top-down length. + */ +pub trait ElevationSampler { + + /** + * Input normalized distance from lower end, get elevation at that point. + */ + fn sample_normalized(&self, top_down_t: f32) -> f32; + + /** + * Input normalized distance from lower end, get the derivative of elevation at that point. + * This is equivalent to the track's grade at this point. + */ + fn grade_normalized(&self, top_down_t: f32) -> f32; + + /** + * Input actual distance in meters from lower end, get elevation at that point. + */ + fn sample_offset(&self, top_down_offset: f32) -> f32; + + /** + * Input actual distance in meters from lower end, get the derivative of elevation at that point. + * This is equivalent to the track's grade at this point. + */ + fn grade_offset(&self, top_down_offset: f32) -> f32; + + /** + * Get the actual length accounting for elevation. + */ + fn length(&self) -> f32; + + /** + * Given an offset relative to the total length of the segment, return the associated distance. + */ + fn to_top_down_normalized(&self, actual_t: f32) -> f32; + + /** + * Given an offset relative to the total length of the segment, return the associated distance. + */ + fn to_top_down_offset(&self, actual_offset: f32) -> f32; +} diff --git a/src/samplers/spline_sampler_3d.rs b/src/samplers/spline_sampler_3d.rs new file mode 100644 index 0000000..11410ed --- /dev/null +++ b/src/samplers/spline_sampler_3d.rs @@ -0,0 +1,34 @@ +use crate::prelude::*; +use crate::samplers::sampler::Sampler3D; + +pub struct SplineSampler3D { + lower_end: Vector3, + lower_control: Vector3, + upper_control: Vector3, + upper_end: Vector3, + bake_interval: f32, +} + +impl Sampler3D { + fn sample(&self, t: f32) -> Vector3 { + let q0 = lower_end + lower_control * t; + let q1 = (lower_end + lower_control).lerp((upper_end + upper_control), t); + let q2 = upper_end + upper_control * (1-t); + + let r0 = q0.lerp(q1, t); + let r1 = q1.lerp(q2, t); + + return r0.lerp(r1, t); + } + + fn tangent(&self, t: f32) -> Vector3 { + let q0 = lower_end + lower_control * t; + let q1 = (lower_end + lower_control).lerp((upper_end + upper_control), t); + let q2 = upper_end + upper_control * (1-t); + + let r0 = q0.lerp(q1, t); + let r1 = q1.lerp(q2, t); + + return r1 - r0; + } +} diff --git a/src/samplers/tangent_sampler.rs b/src/samplers/tangent_sampler.rs new file mode 100644 index 0000000..e426863 --- /dev/null +++ b/src/samplers/tangent_sampler.rs @@ -0,0 +1,368 @@ +use crate::prelude::*; +use crate::samplers::sampler::*; + +pub struct TangentAlignmentSampler { + lower_end: Vector2, + upper_end: Vector2, +} + +impl TangentAlignmentSampler { + pub fn new(lower_end: Vector2, upper_end: Vector2) -> Self { + Self {lower_end, upper_end} + } +} + +impl AlignmentSampler for TangentAlignmentSampler { + fn sample_normalized(&self, t: f32) -> Vector2 { + self.lower_end.lerp(self.upper_end, t) + } + + fn tangent_normalized(&self, _t: f32) -> Vector2 { + (self.upper_end - self.lower_end).normalized() + } + + fn sample_offset(&self, offset: f32) -> Vector2 { + self.sample_normalized(offset / self.length()) + } + + fn tangent_offset(&self, _offset: f32) -> Vector2 { + self.tangent_normalized(0.0) // tangent is constant for this type of alignment + } + + fn length(&self) -> f32 { + (self.upper_end - self.lower_end).length() + } +} + +pub struct TangentElevationSampler { + lower_end: f32, + upper_end: f32, + top_down_length: f32, +} + +impl TangentElevationSampler { + pub fn new(lower_end: f32, upper_end: f32, top_down_length: f32) -> Self { + if top_down_length <= 0.0 { + panic!("TangentElevationSampler cannot have 0 length."); + } + Self { + lower_end, + upper_end, + top_down_length, + } + } +} + +impl ElevationSampler for TangentElevationSampler { + fn sample_normalized(&self, top_down_t: f32) -> f32 { + self.lower_end * (1.0 - top_down_t) + self.upper_end * top_down_t + } + + fn grade_normalized(&self, _top_down_t: f32) -> f32 { + (self.upper_end - self.lower_end) / self.top_down_length + } + + fn sample_offset(&self, top_down_offset: f32) -> f32 { + self.sample_normalized(top_down_offset / self.top_down_length) + } + + fn grade_offset(&self, _top_down_offset: f32) -> f32 { + self.grade_normalized(0.0) + } + + fn length(&self) -> f32 { + let rise = self.upper_end - self.lower_end; + return (rise * rise + self.top_down_length * self.top_down_length).sqrt(); + } + + fn to_top_down_normalized(&self, actual_t: f32) -> f32 { + actual_t + } + + fn to_top_down_offset(&self, actual_offset: f32) -> f32 { + self.top_down_length / self.length() * actual_offset + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_tangent_alignment_sample_offset() { + let test_alignment_1 = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(4.0, 5.0)); + assert_eq!(test_alignment_1.sample_offset(0.0), Vector2::new(1.0, 1.0), + "test_alignment_1 begins in the wrong location."); + assert_eq!(test_alignment_1.sample_offset(5.0), Vector2::new(4.0, 5.0), + "test_alignment_1 ends in the wrong location."); + assert_eq!(test_alignment_1.sample_offset(2.5), Vector2::new(2.5, 3.0), + "test_alignment_1 passes through the wrong location."); + assert_eq!(test_alignment_1.sample_offset(-5.0), Vector2::new(-2.0, -3.0), + "test_alignment_1 negative offset ends in the wrong location."); + assert_eq!(test_alignment_1.sample_offset(10.0), Vector2::new(7.0, 9.0), + "test_alignment_1 double offset ends in the wrong location."); + let test_alignment_2 = TangentAlignmentSampler::new(Vector2::new(-1.0, -1.0), Vector2::new(-4.0, -5.0)); + assert_eq!(test_alignment_2.sample_offset(0.0), Vector2::new(-1.0, -1.0), + "test_alignment_2 begins in the wrong location."); + assert_eq!(test_alignment_2.sample_offset(5.0), Vector2::new(-4.0, -5.0), + "test_alignment_2 ends in the wrong location."); + assert_eq!(test_alignment_2.sample_offset(2.5), Vector2::new(-2.5, -3.0), + "test_alignment_2 passes through the wrong location."); + assert_eq!(test_alignment_2.sample_offset(-5.0), Vector2::new(2.0, 3.0), + "test_alignment_2 negative offset ends in the wrong location."); + assert_eq!(test_alignment_2.sample_offset(10.0), Vector2::new(-7.0, -9.0), + "test_alignment_2 double offset ends in the wrong location."); + } + + #[test] + fn test_tangent_alignment_tangent_offset() { + let test_alignment_1 = TangentAlignmentSampler::new(Vector2::new(0.0, 0.0), Vector2::new(1.0, 0.0)); + let correct_tangent_1 = Vector2::new(1.0, 0.0); + assert_eq!(test_alignment_1.tangent_offset(0.0), correct_tangent_1, + "test_alignment_1 begins with an incorrect tangent."); + assert_eq!(test_alignment_1.tangent_offset(1.0), correct_tangent_1, + "test_alignment_1 ends with an incorrect tangent."); + assert_eq!(test_alignment_1.tangent_offset(0.5), correct_tangent_1, + "test_alignment_1 has an incorrect tangent in the middle."); + assert_eq!(test_alignment_1.tangent_offset(-1.0),correct_tangent_1, + "test_alignment_1 negative offset has an incorrect tangent."); + assert_eq!(test_alignment_1.tangent_offset(2.0), correct_tangent_1, + "test_alignment_1 double offset has an incorrect tangent."); + let test_alignment_2 = TangentAlignmentSampler::new(Vector2::new(0.0, 0.0), Vector2::new(0.0, -1.0)); + let correct_tangent_2 = Vector2::new(0.0, -1.0); + assert_eq!(test_alignment_2.tangent_offset(0.0), correct_tangent_2, + "test_alignment_2 begins with an incorrect tangent."); + assert_eq!(test_alignment_2.tangent_offset(1.0), correct_tangent_2, + "test_alignment_2 ends with an incorrect tangent."); + assert_eq!(test_alignment_2.tangent_offset(0.5), correct_tangent_2, + "test_alignment_2 has an incorrect tangent in the middle."); + assert_eq!(test_alignment_2.tangent_offset(-1.0), correct_tangent_2, + "test_alignment_2 negative offset has an incorrect tangent."); + assert_eq!(test_alignment_2.tangent_offset(2.0), correct_tangent_2, + "test_alignment_2 double offset has an incorrect tangent."); + let test_alignment_3 = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(4.0, 5.0)); + let correct_tangent_3 = Vector2::new(3.0, 4.0).normalized(); + assert_eq!(test_alignment_3.tangent_offset(0.0), correct_tangent_3, + "test_alignment_3 begins with an incorrect tangent."); + assert_eq!(test_alignment_3.tangent_offset(5.0), correct_tangent_3, + "test_alignment_3 ends with an incorrect tangent."); + assert_eq!(test_alignment_3.tangent_offset(2.5), correct_tangent_3, + "test_alignment_3 has an incorrect tangent in the middle."); + assert_eq!(test_alignment_3.tangent_offset(-5.0), correct_tangent_3, + "test_alignment_3 negative offset has an incorrect tangent."); + assert_eq!(test_alignment_3.tangent_offset(10.0), correct_tangent_3, + "test_alignment_3 double offset has an incorrect tangent."); + } + + #[test] + fn test_tangent_alignment_sample_normalized() { + let test_alignment_1 = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(4.0, 5.0)); + assert_eq!(test_alignment_1.sample_normalized(0.0), Vector2::new(1.0, 1.0), + "test_alignment_1 begins in the wrong location."); + assert_eq!(test_alignment_1.sample_normalized(1.0), Vector2::new(4.0, 5.0), + "test_alignment_1 ends in the wrong location."); + assert_eq!(test_alignment_1.sample_normalized(0.5), Vector2::new(2.5, 3.0), + "test_alignment_1 passes through the wrong location."); + assert_eq!(test_alignment_1.sample_normalized(-1.0), Vector2::new(-2.0, -3.0), + "test_alignment_1 negative offset ends in the wrong location."); + assert_eq!(test_alignment_1.sample_normalized(2.0), Vector2::new(7.0, 9.0), + "test_alignment_1 double offset ends in the wrong location."); + let test_alignment_2 = TangentAlignmentSampler::new(Vector2::new(-1.0, -1.0), Vector2::new(-4.0, -5.0)); + assert_eq!(test_alignment_2.sample_normalized(0.0), Vector2::new(-1.0, -1.0), + "test_alignment_2 begins in the wrong location."); + assert_eq!(test_alignment_2.sample_normalized(1.0), Vector2::new(-4.0, -5.0), + "test_alignment_2 ends in the wrong location."); + assert_eq!(test_alignment_2.sample_normalized(0.5), Vector2::new(-2.5, -3.0), + "test_alignment_2 passes through the wrong location."); + assert_eq!(test_alignment_2.sample_normalized(-1.0), Vector2::new(2.0, 3.0), + "test_alignment_2 negative offset ends in the wrong location."); + assert_eq!(test_alignment_2.sample_normalized(2.0), Vector2::new(-7.0, -9.0), + "test_alignment_2 double offset ends in the wrong location."); + } + + #[test] + fn test_tangent_alignment_tangent_normalized() { + let test_alignment_1 = TangentAlignmentSampler::new(Vector2::new(0.0, 0.0), Vector2::new(1.0, 0.0)); + let correct_tangent_1 = Vector2::new(1.0, 0.0); + assert_eq!(test_alignment_1.tangent_normalized(0.0),correct_tangent_1, + "test_alignment_1 begins with an incorrect tangent."); + assert_eq!(test_alignment_1.tangent_normalized(1.0), correct_tangent_1, + "test_alignment_1 ends with an incorrect tangent."); + assert_eq!(test_alignment_1.tangent_normalized(0.5), correct_tangent_1, + "test_alignment_1 has an incorrect tangent in the middle."); + assert_eq!(test_alignment_1.tangent_normalized(-1.0), correct_tangent_1, + "test_alignment_1 negative offset has an incorrect tangent."); + assert_eq!(test_alignment_1.tangent_normalized(2.0), correct_tangent_1, + "test_alignment_1 double offset has an incorrect tangent."); + let test_alignment_2 = TangentAlignmentSampler::new(Vector2::new(0.0, 0.0), Vector2::new(0.0, -1.0)); + let correct_tangent_2 = Vector2::new(0.0, -1.0); + assert_eq!(test_alignment_2.tangent_normalized(0.0), correct_tangent_2, + "test_alignment_2 begins with an incorrect tangent."); + assert_eq!(test_alignment_2.tangent_normalized(1.0), correct_tangent_2, + "test_alignment_2 ends with an incorrect tangent."); + assert_eq!(test_alignment_2.tangent_normalized(0.5), correct_tangent_2, + "test_alignment_2 has an incorrect tangent in the middle."); + assert_eq!(test_alignment_2.tangent_normalized(-1.0), correct_tangent_2, + "test_alignment_2 negative offset has an incorrect tangent."); + assert_eq!(test_alignment_2.tangent_normalized(2.0), correct_tangent_2, + "test_alignment_2 double offset has an incorrect tangent."); + let test_alignment_3 = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(4.0, 5.0)); + let correct_tangent_3 = Vector2::new(3.0, 4.0).normalized(); + assert_eq!(test_alignment_3.tangent_normalized(0.0), correct_tangent_3, + "test_alignment_3 begins with an incorrect tangent."); + assert_eq!(test_alignment_3.tangent_normalized(1.0), correct_tangent_3, + "test_alignment_3 ends with an incorrect tangent."); + assert_eq!(test_alignment_3.tangent_normalized(0.5), correct_tangent_3, + "test_alignment_3 has an incorrect tangent in the middle."); + assert_eq!(test_alignment_3.tangent_normalized(-1.0), correct_tangent_3, + "test_alignment_3 negative offset has an incorrect tangent."); + assert_eq!(test_alignment_3.tangent_normalized(2.0), correct_tangent_3, + "test_alignment_3 double offset has an incorrect tangent."); + } + + #[test] + fn test_tangent_alignment_length() { + let test_alignment_1 = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(4.0, 5.0)); + assert_eq!(test_alignment_1.length(), 5.0, "test_alignment_1 had an incorrect length."); + } + + #[test] + fn test_tangent_elevation_sample_offset() { + let test_elevation_1 = TangentElevationSampler::new(1.0, 2.0, 5.0); + assert_eq!(test_elevation_1.sample_offset(0.0), 1.0, + "test_elevation_1 begins in the wrong location."); + assert_eq!(test_elevation_1.sample_offset(5.0), 2.0, + "test_elevation_1 ends in the wrong location."); + assert_eq!(test_elevation_1.sample_offset(2.5), 1.5, + "test_elevation_1 passes through the wrong location."); + assert_eq!(test_elevation_1.sample_offset(-5.0), 0.0, + "test_elevation_1 negative offset ends in the wrong location."); + assert_eq!(test_elevation_1.sample_offset(10.0), 3.0, + "test_elevation_1 double offset ends in the wrong location."); + let test_elevation_2 = TangentElevationSampler::new(2.0, 2.0, 5.0); + assert_eq!(test_elevation_2.sample_offset(0.0), 2.0, + "test_elevation_2 begins in the wrong location."); + assert_eq!(test_elevation_2.sample_offset(5.0), 2.0, + "test_elevation_2 ends in the wrong location."); + assert_eq!(test_elevation_2.sample_offset(2.5), 2.0, + "test_elevation_2 passes through the wrong location."); + assert_eq!(test_elevation_2.sample_offset(-5.0), 2.0, + "test_elevation_2 negative offset ends in the wrong location."); + assert_eq!(test_elevation_2.sample_offset(10.0), 2.0, + "test_elevation_2 double offset ends in the wrong location."); + } + + #[test] + fn test_tangent_elevation_grade_offset() { + let test_elevation_1 = TangentElevationSampler::new(1.0, 2.0, 5.0); + assert_eq!(test_elevation_1.grade_offset(0.0), 0.2, + "test_elevation_1 begins in the wrong location."); + assert_eq!(test_elevation_1.grade_offset(5.0), 0.2, + "test_elevation_1 ends in the wrong location."); + assert_eq!(test_elevation_1.grade_offset(2.5), 0.2, + "test_elevation_1 passes through the wrong location."); + assert_eq!(test_elevation_1.grade_offset(-5.0), 0.2, + "test_elevation_1 negative offset ends in the wrong location."); + assert_eq!(test_elevation_1.grade_offset(10.0), 0.2, + "test_elevation_1 double offset ends in the wrong location."); + let test_elevation_2 = TangentElevationSampler::new(2.0, 2.0, 5.0); + assert_eq!(test_elevation_2.grade_offset(0.0), 0.0, + "test_elevation_2 begins in the wrong location."); + assert_eq!(test_elevation_2.grade_offset(5.0), 0.0, + "test_elevation_2 ends in the wrong location."); + assert_eq!(test_elevation_2.grade_offset(2.5), 0.0, + "test_elevation_2 passes through the wrong location."); + assert_eq!(test_elevation_2.grade_offset(-5.0), 0.0, + "test_elevation_2 negative offset ends in the wrong location."); + assert_eq!(test_elevation_2.grade_offset(10.0), 0.0, + "test_elevation_2 double offset ends in the wrong location."); + } + + #[test] + fn test_tangent_elevation_sample_normalized() { + let test_elevation_1 = TangentElevationSampler::new(1.0, 2.0, 5.0); + assert_eq!(test_elevation_1.sample_normalized(0.0), 1.0, + "test_elevation_1 begins in the wrong location."); + assert_eq!(test_elevation_1.sample_normalized(1.0), 2.0, + "test_elevation_1 ends in the wrong location."); + assert_eq!(test_elevation_1.sample_normalized(0.5), 1.5, + "test_elevation_1 passes through the wrong location."); + assert_eq!(test_elevation_1.sample_normalized(-1.0), 0.0, + "test_elevation_1 negative offset ends in the wrong location."); + assert_eq!(test_elevation_1.sample_normalized(2.0), 3.0, + "test_elevation_1 double offset ends in the wrong location."); + let test_elevation_2 = TangentElevationSampler::new(2.0, 2.0, 5.0); + assert_eq!(test_elevation_2.sample_normalized(0.0), 2.0, + "test_elevation_2 begins in the wrong location."); + assert_eq!(test_elevation_2.sample_normalized(1.0), 2.0, + "test_elevation_2 ends in the wrong location."); + assert_eq!(test_elevation_2.sample_normalized(0.5), 2.0, + "test_elevation_2 passes through the wrong location."); + assert_eq!(test_elevation_2.sample_normalized(-1.0), 2.0, + "test_elevation_2 negative offset ends in the wrong location."); + assert_eq!(test_elevation_2.sample_normalized(2.0), 2.0, + "test_elevation_2 double offset ends in the wrong location."); + } + + #[test] + fn test_tangent_elevation_grade_normalized() { + let test_elevation_1 = TangentElevationSampler::new(1.0, 2.0, 5.0); + assert_eq!(test_elevation_1.grade_normalized(0.0), 0.2, + "test_elevation_1 begins in the wrong location."); + assert_eq!(test_elevation_1.grade_normalized(1.0), 0.2, + "test_elevation_1 ends in the wrong location."); + assert_eq!(test_elevation_1.grade_normalized(0.5), 0.2, + "test_elevation_1 passes through the wrong location."); + assert_eq!(test_elevation_1.grade_normalized(-1.0), 0.2, + "test_elevation_1 negative offset ends in the wrong location."); + assert_eq!(test_elevation_1.grade_normalized(2.0), 0.2, + "test_elevation_1 double offset ends in the wrong location."); + let test_elevation_2 = TangentElevationSampler::new(2.0, 2.0, 5.0); + assert_eq!(test_elevation_2.grade_normalized(0.0), 0.0, + "test_elevation_2 begins in the wrong location."); + assert_eq!(test_elevation_2.grade_normalized(1.0), 0.0, + "test_elevation_2 ends in the wrong location."); + assert_eq!(test_elevation_2.grade_normalized(0.5), 0.0, + "test_elevation_2 passes through the wrong location."); + assert_eq!(test_elevation_2.grade_normalized(-1.0), 0.0, + "test_elevation_2 negative offset ends in the wrong location."); + assert_eq!(test_elevation_2.grade_normalized(2.0), 0.0, + "test_elevation_2 double offset ends in the wrong location."); + } + + #[test] + fn test_tangent_elevation_length() { + let test_elevation_1 = TangentElevationSampler::new(1.0, 4.0, 4.0); + assert_eq!(test_elevation_1.length(), 5.0, "test_elevation_1 had an incorrect length."); + } + + #[test] + fn test_tangent_elevation_to_top_down_normalized() { + let test_elevation_1 = TangentElevationSampler::new(1.0, 4.0, 4.0); + assert_eq!(test_elevation_1.to_top_down_normalized(0.0), 0.0, + "test_elevation_1 begins in the wrong location."); + assert_eq!(test_elevation_1.to_top_down_normalized(1.0), 1.0, + "test_elevation_1 ends in the wrong location."); + assert_eq!(test_elevation_1.to_top_down_normalized(0.5), 0.5, + "test_elevation_1 passes through the wrong location."); + assert_eq!(test_elevation_1.to_top_down_normalized(-1.0), -1.0, + "test_elevation_1 negative offset ends in the wrong location."); + assert_eq!(test_elevation_1.to_top_down_normalized(2.0), 2.0, + "test_elevation_1 double offset ends in the wrong location."); + } + + #[test] + fn test_tangent_elevation_to_top_down_offset() { + let test_elevation_1 = TangentElevationSampler::new(1.0, 4.0, 4.0); + assert_eq!(test_elevation_1.to_top_down_offset(0.0), 0.0, + "test_elevation_1 begins in the wrong location."); + assert_eq!(test_elevation_1.to_top_down_offset(5.0), 4.0, + "test_elevation_1 ends in the wrong location."); + assert_eq!(test_elevation_1.to_top_down_offset(2.5), 2.0, + "test_elevation_1 passes through the wrong location."); + assert_eq!(test_elevation_1.to_top_down_offset(-5.0), -4.0, + "test_elevation_1 negative offset ends in the wrong location."); + assert_eq!(test_elevation_1.to_top_down_offset(10.0), 8.0, + "test_elevation_1 double offset ends in the wrong location."); + } +} diff --git a/src/segment_endpoint.rs b/src/segment_endpoint.rs new file mode 100644 index 0000000..7077a6f --- /dev/null +++ b/src/segment_endpoint.rs @@ -0,0 +1,13 @@ +pub struct SegmentEndpoint { + internal_segment_index: usize, + is_upper: bool, +} + +impl SegmentEndpoint { + fn new(internal_segment_index: usize, is_upper: bool) -> Self { + Self { + internal_segment_index, + is_upper, + } + } +} diff --git a/src/simple_segment.rs b/src/simple_segment.rs new file mode 100644 index 0000000..d4cf2bb --- /dev/null +++ b/src/simple_segment.rs @@ -0,0 +1,379 @@ +use crate::prelude::*; +use crate::samplers::sampler::*; +use crate::rail_segment; +use crate::rail_segment::RailSegment; +use crate::rail_connector::RailConnector; + +/** + * Pure rust implementation of Basis::new_looking_at intended to allow unit tests. + */ +fn new_looking_at(direction: Vector3, up: Vector3, _positive_z_forward: bool) -> Basis { + let z = -direction.normalized(); + let x = up.cross(z).normalized(); + let y = z.cross(x); + Basis::from_cols(x, y, z) +} + +pub struct SimpleSegment { + alignment: A, + elevation: E, + + lower_connection: Option, + upper_connection: Option, + //superelevation: Box, // Sometime I may get around to making this. + + // cache some stuff + //endpoint0: Option, + //endpoint1: Option, + //length: Option, +} + +impl SimpleSegment { + pub fn new(alignment: A, elevation: E) -> Self { + Self { + alignment, + elevation, + lower_connection: Option::None, + upper_connection: Option::None, + //endpoint0: None, + //endpoint1: None, + //length: None, + } + } +} + +impl RailSegment for SimpleSegment { + fn get_length(&self, _index: usize) -> Result { + /*match self.length { + Some(length) => Ok(length), + None => { + let result = self.elevation.length(); + self.length = Some(result); + Ok(result) + }, + }*/ + Ok(self.elevation.length()) + } + + fn sample(&self, _index: usize, offset: f32) -> Result { + if offset < 0.0 || offset > self.get_length(1).unwrap() { + return Err(rail_segment::Error::OffsetError); + } + let top_down_t = self.elevation.to_top_down_normalized(offset / self.elevation.length()); + let xz = self.alignment.sample_normalized(top_down_t); + let y = self.elevation.sample_normalized(top_down_t); + Ok(Vector3::new(xz.x, y, xz.y)) + } + + fn sample_with_rotation(&self, _index: usize, offset: f32) -> Result { + if offset < 0.0 || offset > self.get_length(1).unwrap() { + return Err(rail_segment::Error::OffsetError); + } + let top_down_t = self.elevation.to_top_down_normalized(offset / self.elevation.length()); + let direction_xz = self.alignment.tangent_normalized(top_down_t); + let direction_y = self.elevation.grade_normalized(top_down_t); + let direction = Vector3::new(direction_xz.x, direction_y, direction_xz.y); + let basis = new_looking_at(direction, Vector3::UP, false); + let xz = self.alignment.sample_normalized(top_down_t); + let y = self.elevation.sample_normalized(top_down_t); + Ok(Transform3D::new(basis, Vector3::new(xz.x, y, xz.y))) + } + + fn sample_with_rotation_reverse(&self, _index: usize, offset: f32) -> Result { + if offset < 0.0 || offset > self.get_length(1).unwrap() { + return Err(rail_segment::Error::OffsetError); + } + let top_down_t = self.elevation.to_top_down_normalized(offset / self.elevation.length()); + let direction_zx = self.alignment.tangent_normalized(top_down_t); + let direction_y = self.elevation.grade_normalized(top_down_t); + let direction = Vector3::new(-direction_zx.x, -direction_y, -direction_zx.y); + let basis = new_looking_at(direction, Vector3::UP, false); + let xz = self.alignment.sample_normalized(top_down_t); + let y = self.elevation.sample_normalized(top_down_t); + Ok(Transform3D::new(basis, Vector3::new(xz.x, y, xz.y))) + } + + fn sample_up_vector(&self, _index: usize, offset: f32) -> Result { + if offset < 0.0 || offset > self.get_length(1).unwrap() { + return Err(rail_segment::Error::OffsetError); + } + let top_down_t = self.elevation.to_top_down_normalized(offset / self.elevation.length()); + let direction_xz = self.alignment.tangent_normalized(top_down_t); + let direction_y = self.elevation.grade_normalized(top_down_t); + let direction = Vector3::new(direction_xz.x, direction_y, direction_xz.y); + let normal = Vector3::new(direction_xz.y, 0.0, -direction_xz.x); + Ok(direction.cross(normal).normalized()) + } + + fn segment_from_endpoint(&self, _endpoint: usize) -> Result { + Ok(1) + } + + fn get_endpoint_transform(&self, endpoint: usize) -> Result { + match endpoint { + /*0 => { + match self.endpoint0 { + Some(result) => Ok(result), + None => { + let result = self.sample_with_rotation_reverse(0, 0.0).unwrap(); + self.endpoint0 = Some(result); + Ok(result) + }, + } + }, + 1 => { + match self.endpoint1 { + Some(result) => Ok(result), + None => { + let result = self.sample_with_rotation(0, self.get_length(1).unwrap()).unwrap(); + self.endpoint1 = Some(result); + Ok(result) + }, + } + },*/ + 0 => self.sample_with_rotation_reverse(0, 0.0), + 1 => self.sample_with_rotation(0, self.get_length(1).unwrap()), + _ => Err(rail_segment::Error::EndpointError), + } + } + + fn is_endpoint_upper(&self, endpoint: usize) -> Result { + match endpoint { + 0 => Ok(false), + 1 => Ok(true), + _ => Err(rail_segment::Error::EndpointError), + } + } + + fn num_endpoints(&self) -> usize { + 2 + } + + fn borrow_connection(&self, endpoint: usize) -> Result<&RailConnector, rail_segment::Error> { + match endpoint { + 0 => { + if let Some(ret) = &self.lower_connection { + Ok(ret) + } else { + Err(rail_segment::Error::NeighborError) + } + }, + 1 => { + if let Some(ret) = &self.upper_connection { + Ok(ret) + } else { + Err(rail_segment::Error::NeighborError) + } + }, + _ => Err(rail_segment::Error::EndpointError), + } + } + + fn check_connection(&self, endpoint: usize) -> Result { + match endpoint { + 0 => { + match &self.lower_connection { + Some(_) => Ok(true), + None => Ok(false), + } + }, + 1 => { + match &self.upper_connection { + Some(_) => Ok(true), + None => Ok(false), + } + }, + _ => Err(rail_segment::Error::EndpointError), + } + } + + fn connection_iter(&self) -> rail_segment::ConnectionIter { + rail_segment::ConnectionIter::new(self) + } + + fn add_connection(&mut self, endpoint: usize, connector: RailConnector) -> Result { + match endpoint { + 0 => { + self.lower_connection = Option::Some(connector); + Ok(true) + }, + 1 => { + self.upper_connection = Option::Some(connector); + Ok(true) + }, + _ => Err(rail_segment::Error::EndpointError), + } + } + + fn remove_connection(&mut self, endpoint: usize) -> Result { + match endpoint { + 0 => { + self.lower_connection = Option::None; + Ok(true) + }, + 1 => { + self.upper_connection = Option::None; + Ok(true) + }, + _ => Err(rail_segment::Error::EndpointError), + } + } + + fn remove_all_connections(&mut self) { + self.lower_connection = Option::None; + self.upper_connection = Option::None; + } + + fn get_state(&self) -> f32 { + 0.0 + } + + fn set_state(&mut self, _: f32) { + // This does nothing for simple segments. + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::samplers::tangent_sampler::*; + + fn compare_vectors(vec1: Vector3, vec2: Vector3, epsilon: f32, message: &str) { + assert!((vec1.x - vec2.x).abs() <= epsilon && (vec1.y - vec2.y).abs() <= epsilon, + "{message} Expected {vec2}, received {vec1}.") + } + + /*fn compare_floats(float1: f32, float2: f32, epsilon: f32, message: &str) { + assert!((float1 - float2).abs() <= epsilon, + "{message} Expected {float2}, received {float1}.") + }*/ + + #[test] + fn test_basis_new_looking_at() { + let test_basis = new_looking_at(Vector3::FORWARD, Vector3::UP, false); + assert_eq!(test_basis.col_a(), Vector3::RIGHT, + "Column A was incorrect."); + assert_eq!(test_basis.col_b(), Vector3::UP, + "Column B was incorrect."); + assert_eq!(test_basis.col_c(), Vector3::BACK, + "Column C was incorrect."); + } + + #[test] + fn test_get_length() { + let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0)); + let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0); + let test_segment = SimpleSegment::new(alignment, elevation); + assert_eq!(test_segment.get_length(1).unwrap(), 17.0, + "Segment length was incorrect."); + } + + #[test] + fn test_sample() { + let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0)); + let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0); + let test_segment = SimpleSegment::new(alignment, elevation); + assert_eq!(test_segment.sample(1, 0.0).unwrap(), Vector3::new(1.0, 1.0, 1.0), + "Segment begins at the wrong location."); + assert_eq!(test_segment.sample(1, 17.0).unwrap(), Vector3::new(10.0, 9.0, 13.0), + "Segment ends at the wrong location."); + assert_eq!(test_segment.sample(1, 8.5).unwrap(), Vector3::new(5.5, 5.0, 7.0), + "Segment passes through the wrong location."); + } + + #[test] + fn test_sample_with_rotation() { + let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0)); + let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0); + let test_segment = SimpleSegment::new(alignment, elevation); + let direction_forward = Vector3::new(-9.0 / 17.0, -8.0 / 17.0, -12.0 / 17.0); + let direction_right = Vector3::new(-0.8, 0.0, 0.6); + let direction_up = direction_forward.cross(direction_right); + let transform1 = test_segment.sample_with_rotation(1, 0.0).unwrap(); + let epsilon = 1e-6; + assert_eq!(transform1.origin, Vector3::new(1.0, 1.0, 1.0), + "Segment begins at the wrong location."); + compare_vectors(transform1.basis.col_c(), direction_forward, epsilon, + "Segment begins facing the wrong direction."); + compare_vectors(transform1.basis.col_a(), direction_right, epsilon, + "Segment begins with the wrong right direction."); + compare_vectors(transform1.basis.col_b(), direction_up, epsilon, + "Segment begins with the wrong up direction."); + let transform2 = test_segment.sample_with_rotation(1, 17.0).unwrap(); + assert_eq!(transform2.origin, Vector3::new(10.0, 9.0, 13.0), + "Segment ends at the wrong location."); + compare_vectors(transform2.basis.col_c(), direction_forward, epsilon, + "Segment ends facing the wrong direction."); + compare_vectors(transform2.basis.col_a(), direction_right, epsilon, + "Segment ends with the wrong righ direction."); + compare_vectors(transform2.basis.col_b(), direction_up, epsilon, + "Segment ends with the wrong up direction."); + let transform3 = test_segment.sample_with_rotation(1, 8.5).unwrap(); + assert_eq!(transform3.origin, Vector3::new(5.5, 5.0, 7.0), + "Segment passes through the wrong location."); + compare_vectors(transform3.basis.col_c(), direction_forward, epsilon, + "Segment passes through facing the wrong direction."); + compare_vectors(transform3.basis.col_a(), direction_right, epsilon, + "Segment passes through with the wrong righ direction."); + compare_vectors(transform3.basis.col_b(), direction_up, epsilon, + "Segment passes through with the wrong up direction."); + } + + #[test] + fn test_sample_with_rotation_reverse() { + let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0)); + let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0); + let test_segment = SimpleSegment::new(alignment, elevation); + let direction_forward = Vector3::new(9.0 / 17.0, 8.0 / 17.0, 12.0 / 17.0); + let direction_right = Vector3::new(0.8, 0.0, -0.6); + let direction_up = direction_forward.cross(direction_right); + let transform1 = test_segment.sample_with_rotation_reverse(1, 0.0).unwrap(); + let epsilon = 1e-6; + assert_eq!(transform1.origin, Vector3::new(1.0, 1.0, 1.0), + "Segment begins at the wrong location."); + compare_vectors(transform1.basis.col_c(), direction_forward, epsilon, + "Segment begins facing the wrong direction."); + compare_vectors(transform1.basis.col_a(), direction_right, epsilon, + "Segment begins with the wrong right direction."); + compare_vectors(transform1.basis.col_b(), direction_up, epsilon, + "Segment begins with the wrong up direction."); + let transform2 = test_segment.sample_with_rotation_reverse(1, 17.0).unwrap(); + assert_eq!(transform2.origin, Vector3::new(10.0, 9.0, 13.0), + "Segment ends at the wrong location."); + compare_vectors(transform2.basis.col_c(), direction_forward, epsilon, + "Segment ends facing the wrong direction."); + compare_vectors(transform2.basis.col_a(), direction_right, epsilon, + "Segment ends with the wrong righ direction."); + compare_vectors(transform2.basis.col_b(), direction_up, epsilon, + "Segment ends with the wrong up direction."); + let transform3 = test_segment.sample_with_rotation_reverse(1, 8.5).unwrap(); + assert_eq!(transform3.origin, Vector3::new(5.5, 5.0, 7.0), + "Segment passes through the wrong location."); + compare_vectors(transform3.basis.col_c(), direction_forward, epsilon, + "Segment passes through facing the wrong direction."); + compare_vectors(transform3.basis.col_a(), direction_right, epsilon, + "Segment passes through with the wrong righ direction."); + compare_vectors(transform3.basis.col_b(), direction_up, epsilon, + "Segment passes through with the wrong up direction."); + } + + #[test] + fn test_sample_up_vector() { + let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0)); + let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0); + let test_segment = SimpleSegment::new(alignment, elevation); + let direction_forward = Vector3::new(-9.0 / 17.0, -8.0 / 17.0, -12.0 / 17.0); + let direction_right = Vector3::new(-0.8, 0.0, 0.6); + let direction_up = direction_forward.cross(direction_right); + let up1 = test_segment.sample_up_vector(1, 0.0).unwrap(); + let epsilon = 1e-6; + compare_vectors(up1, direction_up, epsilon, + "Segment begins with the wrong up direction."); + let up2 = test_segment.sample_up_vector(1, 17.0).unwrap(); + compare_vectors(up2, direction_up, epsilon, + "Segment ends with the wrong up direction."); + let up3 = test_segment.sample_up_vector(1, 8.5).unwrap(); + compare_vectors(up3, direction_up, epsilon, + "Segment passes through with the wrong up direction."); + } +}