Fleshed out samplers, segments, and the rail graph.

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<A: AlignmentSampler, E: ElevationSampler> {
+    alignment: A,
+    elevation: E,
+    
+    lower_connection: Option<RailConnector>,
+    upper_connection: Option<RailConnector>,
+    //superelevation: Box<dyn SuperElevationSampler>, // Sometime I may get around to making this.
+    
+    // cache some stuff
+    //endpoint0: Option<Transform3D>,
+    //endpoint1: Option<Transform3D>,
+    //length: Option<f32>,
+}
+
+impl<A: AlignmentSampler, E: ElevationSampler> SimpleSegment<A, E> {
+    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<A: AlignmentSampler, E: ElevationSampler> RailSegment for SimpleSegment<A, E> {
+    fn get_length(&self, _index: usize) -> Result<f32, rail_segment::Error> {
+        /*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<Vector3, rail_segment::Error> {
+        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<Transform3D, rail_segment::Error> {
+        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<Transform3D, rail_segment::Error> {
+        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<Vector3, rail_segment::Error> {
+        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<usize, rail_segment::Error> {
+        Ok(1)
+    }
+    
+    fn get_endpoint_transform(&self, endpoint: usize) -> Result<Transform3D, rail_segment::Error> {
+        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<bool, rail_segment::Error> {
+        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<bool, rail_segment::Error> {
+        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<bool, rail_segment::Error> {
+        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<bool, rail_segment::Error> {
+        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.");
+    }
+}