Added functions to query and traverse using GraphCoords.

src/graph_coord.rs

@@ -0,0 +1,20 @@
+#[derive(Copy, Clone)]
+pub struct GraphCoord {
+    pub segment:    usize,
+    pub subsegment: usize,
+    pub offset:     f32,
+    pub forward:    bool, // I may change this to a number and use the sign to determine direction.
+    pub derailed:   bool,
+}
+
+impl GraphCoord {
+    pub fn new(segment: usize, subsegment: usize, offset: f32, forward: bool, derailed: bool) -> Self {
+        Self {
+            segment,
+            subsegment,
+            offset,
+            forward,
+            derailed,
+        }
+    }
+}

src/lib.rs

@@ -1,4 +1,4 @@
-//mod graph_coord;
+mod graph_coord;
 mod rail_graph;
 mod rail_segment;
 mod rail_connector;

src/rail_graph.rs

@@ -2,7 +2,7 @@ use crate::prelude::*;
 use crate::rail_segment;
 use crate::rail_segment::RailSegment;
 use crate::rail_connector::RailConnector;
-//use crate::graph_coord::GraphCoord;
+use crate::graph_coord::GraphCoord;
 //use std::collections::HashMap;
 use std::vec::Vec;
 //use std::string::String;
@@ -181,9 +181,45 @@ impl RailGraph {
     
     // querying and traversal
     
-    //pub fn transform_of_coord(&self, coord: &GraphCoord) -> Transform3D {
-    //    if let Some(segment) = self.
-    //}
+    /**
+     * Get a transform as described by the given coordinate.
+     */
+    pub fn transform_of_coord(&self, coord: &GraphCoord) -> Result<Transform3D, Error> {
+        match self.get_segment(coord.segment) {
+            Ok(segment) => {
+                let ret = match coord.forward {
+                    true => segment.sample_with_rotation(coord.subsegment, coord.offset),
+                    false => segment.sample_with_rotation_reverse(coord.subsegment, coord.offset),
+                };
+                match ret {
+                    Ok(tf) => Ok(tf),
+                    Err(e) => Err(Error::SegmentError(e)),
+                }
+            },
+            Err(e) => Err(e),
+        }
+    }
+    
+    /**
+     * Get a coordinate defined by the previous coordinate, moved by an offset.
+     * The offset can be negative and is relative to @coord.forward.
+     */
+    pub fn traverse(&self, coord: &GraphCoord, offset: f32) -> Result<GraphCoord, Error> {
+        match self.get_segment(coord.segment) {
+            Ok(segment) => {
+                if segment.has_subsegment(coord.subsegment) {
+                    let new_offset = coord.offset + match coord.forward {
+                        true => offset,
+                        false => -offset,
+                    };
+                    Ok(self.traverse_p(segment, coord.subsegment, coord.subsegment, new_offset, coord.forward))
+                } else {
+                    Err(Error::SegmentError(rail_segment::Error::IndexError(coord.subsegment)))
+                }
+            },
+            Err(e) => Err(e),
+        }
+    }
     
     // private
     
@@ -198,8 +234,8 @@ impl RailGraph {
         match self.get_segment(index) {
             Ok(segment) => {
                 match segment.check_connection(endpoint) {
-                    Err(rail_segment::Error::EndpointError(endpoint)) => false,
-                    _                                                 => true,
+                    Err(rail_segment::Error::EndpointError(_)) => false,
+                    _                                          => true,
                 }
             },
             Err(_)      => false,
@@ -243,10 +279,85 @@ impl RailGraph {
     }
     
     fn update_min_available_index(&mut self) {
+        // I should just replace this with a heap.
         while let Some(_segment) = &self.segments[self.min_available_index] {
             self.min_available_index += 1;
         }
     }
+    
+    // Difference from the public version: offset is relative to the segment.
+    // Segment index and segment both being arguments seems pretty redundant,
+    // but the index isn't contained by the segment itself.
+    // Maybe that should change.
+    fn traverse_p(&self, segment: &Box<dyn RailSegment>, segment_index: usize, subsegment_index: usize, offset: f32, forward: bool) -> GraphCoord {
+        // Use unwraps when I can because at this point, the graph should be valid.
+        let segment_length = segment.get_length(subsegment_index).unwrap();
+        if offset > segment_length {
+            // We're off the upper end of the segment, so go to its neighbor.
+            match segment.endpoint_from_subsegment(subsegment_index, true) {
+                Ok(endpoint) => {
+                    match segment.borrow_connection(endpoint) {
+                        Ok(next_connection) => {
+                            let next_segment = self.get_segment(next_connection.segment_index).unwrap();
+                            let next_subsegment = next_segment.subsegment_from_endpoint(next_connection.endpoint_index).unwrap();
+                            let next_endpoint_upper = next_segment.is_endpoint_upper(next_connection.endpoint_index).unwrap();
+                            let next_forward = forward != next_endpoint_upper;
+                            let next_offset = match next_endpoint_upper {
+                                true => {
+                                    let next_length = next_segment.get_length(next_subsegment).unwrap();
+                                    next_length + segment_length - offset
+                                },
+                                false => offset - segment_length,
+                            };
+                            // Recurse with the next segment.
+                            self.traverse_p(next_segment, next_connection.segment_index, next_subsegment, next_offset, next_forward)
+                        },
+                        Err(_) => {
+                            // No connected segment, derail.
+                            GraphCoord::new(segment_index, subsegment_index, offset, forward, true)
+                        },
+                    }
+                },
+                Err(_) => {
+                    // Subsegment doesn't meet an endpoint, derail.
+                    GraphCoord::new(segment_index, subsegment_index, offset, forward, true)
+                },
+            }
+        } else if offset < 0.0 {
+            // We're off the lower end of the segment, so go to its neighbor.
+            match segment.endpoint_from_subsegment(subsegment_index, false) {
+                Ok(endpoint) => {
+                    match segment.borrow_connection(endpoint) {
+                        Ok(next_connection) => {
+                            let next_segment = self.get_segment(next_connection.segment_index).unwrap();
+                            let next_subsegment = next_segment.subsegment_from_endpoint(next_connection.endpoint_index).unwrap();
+                            let next_endpoint_upper = next_segment.is_endpoint_upper(next_connection.endpoint_index).unwrap();
+                            let next_forward = forward == next_endpoint_upper;
+                            let next_offset = match next_endpoint_upper {
+                                true => {
+                                    let next_length = next_segment.get_length(next_subsegment).unwrap();
+                                    next_length + offset
+                                },
+                                false => -offset,
+                            };
+                            // Recurse with the next segment.
+                            self.traverse_p(next_segment, next_connection.segment_index, next_subsegment, next_offset, next_forward)
+                        },
+                        Err(_) => {
+                            // No connected segment, derail.
+                            GraphCoord::new(segment_index, subsegment_index, offset, forward, true)
+                        },
+                    }
+                },
+                Err(_) => {
+                    // Subsegment doesn't meet an endpoint, derail.
+                    GraphCoord::new(segment_index, subsegment_index, offset, forward, true)
+                },
+            }
+        } else {
+            GraphCoord::new(segment_index, subsegment_index, offset, forward, false)
+        }
+    }
 }
 
 #[cfg(test)]
@@ -444,4 +555,144 @@ mod tests {
         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.");
     }
+    
+    #[test]
+    fn test_transform_of_coord() {
+        let test_graph = create_test_graph();
+        let first_segment = test_graph.get_segment(0).unwrap();
+        
+        let coord1 = GraphCoord::new(0, 0, 0.5, true, false);
+        let truth1 = first_segment.sample_with_rotation(0, 0.5).unwrap();
+        let tf1 = test_graph.transform_of_coord(&coord1).unwrap();
+        assert_eq!(tf1.origin, truth1.origin, "transform_of_coord origin comparison 1 failed.");
+        assert_eq!(tf1.basis, truth1.basis, "transform_of_coord basis comparison 1 failed.");
+        
+        let coord2 = GraphCoord::new(0, 0, 0.5, false, false);
+        let truth2 = first_segment.sample_with_rotation_reverse(0, 0.5).unwrap();
+        let tf2 = test_graph.transform_of_coord(&coord2).unwrap();
+        assert_eq!(tf2.origin, truth2.origin, "transform_of_coord origin comparison 2 failed.");
+        assert_eq!(tf2.basis, truth2.basis, "transform_of_coord basis comparison 2 failed.");
+        
+        let coord3 = GraphCoord::new(3, 0, 0.5, true, false);
+        let tf3 = test_graph.transform_of_coord(&coord3);
+        assert!(match tf3 { Result::Err(Error::IndexError(3))=>true, _=>false },
+                "Retrieved the transform of a coordinate with an invalid segment.");
+        
+        let coord4 = GraphCoord::new(0, 1, 0.5, true, false);
+        let tf4 = test_graph.transform_of_coord(&coord4);
+        assert!(match tf4 { Result::Err(Error::SegmentError(rail_segment::Error::IndexError(1)))=>true, _=>false },
+                "Retrieved the transform of a coordinate with an invalid subsegment.");
+        
+        let coord5 = GraphCoord::new(0, 0, 1.5, true, false);
+        let tf5 = test_graph.transform_of_coord(&coord5);
+        assert!(match tf5 { Result::Err(Error::SegmentError(rail_segment::Error::OffsetError(1.5)))=>true, _=>false },
+                "Retrieved the transform of a coordinate with an invalid offset.");
+    }
+    
+    #[test]
+    fn test_traverse() {
+        // make a simple graph with 4 straight segments
+        // Segments 0, 1, and 3 are "forward" and 2 is "backward".
+        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 = TangentAlignmentSampler::new(Vector2::new(1.0, 0.0), Vector2::new(2.0, 0.0));
+        let elevation2 = TangentElevationSampler::new(0.0, 0.0, 1.0);
+        let test_segment2 = Box::new(SimpleSegment::new(alignment2, elevation2));
+        let alignment3 = TangentAlignmentSampler::new(Vector2::new(3.0, 0.0), Vector2::new(2.0, 0.0));
+        let elevation3 = TangentElevationSampler::new(0.0, 0.0, 1.0);
+        let test_segment3 = Box::new(SimpleSegment::new(alignment3, elevation3));
+        let alignment4 = TangentAlignmentSampler::new(Vector2::new(31.0, 0.0), Vector2::new(4.0, 0.0));
+        let elevation4 = TangentElevationSampler::new(0.0, 0.0, 1.0);
+        let test_segment4 = Box::new(SimpleSegment::new(alignment4, elevation4));
+        
+        let mut test_graph = RailGraph::new();
+        test_graph.add_segment(test_segment1);
+        let _ = test_graph.append_segment(test_segment2, 0, 0, 1);
+        let _ = test_graph.append_segment(test_segment3, 1, 1, 1);
+        let _ = test_graph.append_segment(test_segment4, 0, 2, 0);
+        
+        //let first_segment = test_graph.get_segment(0).unwrap();
+        
+        let coord1 = GraphCoord::new(0, 0, 0.25, true, false);
+        let truth1 = GraphCoord::new(0, 0, 0.75, true, false);
+        let ret1 = test_graph.traverse(&coord1, 0.5).unwrap();
+        assert_eq!(ret1.segment, truth1.segment, "Incorrect segment in test 1.");
+        assert_eq!(ret1.subsegment, truth1.subsegment, "Incorrect subsegment in test 1.");
+        assert_eq!(ret1.offset, truth1.offset, "Incorrect offset in test 1.");
+        assert_eq!(ret1.forward, truth1.forward, "Incorrect forward in test 1.");
+        assert_eq!(ret1.derailed, truth1.derailed, "Incorrect derailed in test 1.");
+        
+        let coord2 = GraphCoord::new(0, 0, 0.75, false, false);
+        let truth2 = GraphCoord::new(0, 0, 0.25, false, false);
+        let ret2 = test_graph.traverse(&coord2, 0.5).unwrap();
+        assert_eq!(ret2.segment, truth2.segment, "Incorrect segment in test 2.");
+        assert_eq!(ret2.subsegment, truth2.subsegment, "Incorrect subsegment in test 2.");
+        assert_eq!(ret2.offset, truth2.offset, "Incorrect offset in test 2.");
+        assert_eq!(ret2.forward, truth2.forward, "Incorrect forward in test 2.");
+        assert_eq!(ret2.derailed, truth2.derailed, "Incorrect derailed in test 2.");
+        
+        let coord3 = GraphCoord::new(0, 0, 0.25, true, false);
+        let truth3 = GraphCoord::new(3, 0, 0.25, true, false);
+        let ret3 = test_graph.traverse(&coord3, 3.0).unwrap();
+        assert_eq!(ret3.segment, truth3.segment, "Incorrect segment in test 3.");
+        assert_eq!(ret3.subsegment, truth3.subsegment, "Incorrect subsegment in test 3.");
+        assert_eq!(ret3.offset, truth3.offset, "Incorrect offset in test 3.");
+        assert_eq!(ret3.forward, truth3.forward, "Incorrect forward in test 3.");
+        assert_eq!(ret3.derailed, truth3.derailed, "Incorrect derailed in test 3.");
+        
+        let coord4 = GraphCoord::new(3, 0, 0.25, false, false);
+        let truth4 = GraphCoord::new(0, 0, 0.25, false, false);
+        let ret4 = test_graph.traverse(&coord4, 3.0).unwrap();
+        assert_eq!(ret4.segment, truth4.segment, "Incorrect segment in test 4.");
+        assert_eq!(ret4.subsegment, truth4.subsegment, "Incorrect subsegment in test 4.");
+        assert_eq!(ret4.offset, truth4.offset, "Incorrect offset in test 4.");
+        assert_eq!(ret4.forward, truth4.forward, "Incorrect forward in test 4.");
+        assert_eq!(ret4.derailed, truth4.derailed, "Incorrect derailed in test 4.");
+        
+        let coord5 = GraphCoord::new(1, 0, 0.25, false, false);
+        let truth5 = GraphCoord::new(2, 0, 0.75, true, false);
+        let ret5 = test_graph.traverse(&coord5, -1.0).unwrap();
+        assert_eq!(ret5.segment, truth5.segment, "Incorrect segment in test 5.");
+        assert_eq!(ret5.subsegment, truth5.subsegment, "Incorrect subsegment in test 5.");
+        assert_eq!(ret5.offset, truth5.offset, "Incorrect offset in test 5.");
+        assert_eq!(ret5.forward, truth5.forward, "Incorrect forward in test 5.");
+        assert_eq!(ret5.derailed, truth5.derailed, "Incorrect derailed in test 5.");
+        
+        let coord5 = GraphCoord::new(2, 0, 0.25, false, false);
+        let truth5 = GraphCoord::new(3, 0, 0.75, true, false);
+        let ret5 = test_graph.traverse(&coord5, 1.0).unwrap();
+        assert_eq!(ret5.segment, truth5.segment, "Incorrect segment in test 5.");
+        assert_eq!(ret5.subsegment, truth5.subsegment, "Incorrect subsegment in test 5.");
+        assert_eq!(ret5.offset, truth5.offset, "Incorrect offset in test 5.");
+        assert_eq!(ret5.forward, truth5.forward, "Incorrect forward in test 5.");
+        assert_eq!(ret5.derailed, truth5.derailed, "Incorrect derailed in test 5.");
+        
+        let coord6 = GraphCoord::new(3, 0, 0.25, false, false);
+        let truth6 = GraphCoord::new(2, 0, 0.75, true, false);
+        let ret6 = test_graph.traverse(&coord6, 1.0).unwrap();
+        assert_eq!(ret6.segment, truth6.segment, "Incorrect segment in test 6.");
+        assert_eq!(ret6.subsegment, truth6.subsegment, "Incorrect subsegment in test 6.");
+        assert_eq!(ret6.offset, truth6.offset, "Incorrect offset in test 6.");
+        assert_eq!(ret6.forward, truth6.forward, "Incorrect forward in test 6.");
+        assert_eq!(ret6.derailed, truth6.derailed, "Incorrect derailed in test 6.");
+        
+        let coord7 = GraphCoord::new(2, 0, 0.25, false, false);
+        let truth7 = GraphCoord::new(1, 0, 0.75, true, false);
+        let ret7 = test_graph.traverse(&coord7, -1.0).unwrap();
+        assert_eq!(ret7.segment, truth7.segment, "Incorrect segment in test 7.");
+        assert_eq!(ret7.subsegment, truth7.subsegment, "Incorrect subsegment in test 7.");
+        assert_eq!(ret7.offset, truth7.offset, "Incorrect offset in test 7.");
+        assert_eq!(ret7.forward, truth7.forward, "Incorrect forward in test 7.");
+        assert_eq!(ret7.derailed, truth7.derailed, "Incorrect derailed in test 7.");
+        
+        let coord8 = GraphCoord::new(0, 0, 0.25, true, false);
+        let truth8 = GraphCoord::new(3, 0, 1.25, true, true);
+        let ret8 = test_graph.traverse(&coord8, 4.0).unwrap();
+        assert_eq!(ret8.segment, truth8.segment, "Incorrect segment in test 8.");
+        assert_eq!(ret8.subsegment, truth8.subsegment, "Incorrect subsegment in test 8.");
+        assert_eq!(ret8.offset, truth8.offset, "Incorrect offset in test 8.");
+        assert_eq!(ret8.forward, truth8.forward, "Incorrect forward in test 8.");
+        assert_eq!(ret8.derailed, truth8.derailed, "Incorrect derailed in test 8.");
+    }
 }

src/rail_segment.rs

@@ -89,10 +89,17 @@ pub trait RailSegment {
      */
     fn sample_up_vector(&self, index: usize, offset: f32) -> Result<Vector3, Error>;
     
+    /** Check if a subsegment is valid.
+     */
+    fn has_subsegment(&self, index: usize) -> bool;
+    
     // endpoint functions
     
     /** Get the segment index associated with an endpoint. */
-    fn segment_from_endpoint(&self, endpoint: usize) -> Result<usize, Error>;
+    fn subsegment_from_endpoint(&self, endpoint: usize) -> Result<usize, Error>;
+    
+    /** Get the segment index associated with an endpoint. */
+    fn endpoint_from_subsegment(&self, index: usize, is_upper: bool) -> Result<usize, Error>;
     
     /** Get the location of an endpoint. */
     fn get_endpoint_transform(&self, endpoint: usize) -> Result<Transform3D, Error>;

src/simple_segment.rs

@@ -116,7 +116,11 @@ impl<A: AlignmentSampler, E: ElevationSampler> RailSegment for SimpleSegment<A,
         Ok(direction.cross(normal).normalized())
     }
     
-    fn segment_from_endpoint(&self, endpoint: usize) -> Result<usize, rail_segment::Error> {
+    fn has_subsegment(&self, index: usize) -> bool {
+        index == 0
+    }
+    
+    fn subsegment_from_endpoint(&self, endpoint: usize) -> Result<usize, rail_segment::Error> {
         if endpoint > 1 {
             return Err(rail_segment::Error::EndpointError(endpoint));
         } else {
@@ -124,6 +128,18 @@ impl<A: AlignmentSampler, E: ElevationSampler> RailSegment for SimpleSegment<A,
         }
     }
     
+    fn endpoint_from_subsegment(&self, index: usize, is_upper: bool) -> Result<usize, rail_segment::Error> {
+        match index {
+            0 => {
+                match is_upper {
+                    true  => Ok(1),
+                    false => Ok(0),
+                }
+            },
+            _ => Err(rail_segment::Error::IndexError(index)),
+        }
+    }
+    
     fn get_endpoint_transform(&self, endpoint: usize) -> Result<Transform3D, rail_segment::Error> {
         match endpoint {
             0 => self.sample_with_rotation_reverse(0, 0.0),
@@ -439,17 +455,17 @@ mod tests {
     }
     
     #[test]
-    fn test_segment_from_endpoint() {
+    fn test_subsegment_from_endpoint() {
         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);
         
         // test successes
-        assert_eq!(test_segment.segment_from_endpoint(0).unwrap(), 0, "Endpoint 0 points to incorrect internal segment.");
-        assert_eq!(test_segment.segment_from_endpoint(1).unwrap(), 0, "Endpoint 1 points to incorrect internal segment.");
+        assert_eq!(test_segment.subsegment_from_endpoint(0).unwrap(), 0, "Endpoint 0 points to incorrect internal segment.");
+        assert_eq!(test_segment.subsegment_from_endpoint(1).unwrap(), 0, "Endpoint 1 points to incorrect internal segment.");
         
         // test failures
-        assert!(match test_segment.segment_from_endpoint(2) { Err(rail_segment::Error::EndpointError(2))=>true, _=>false, },
+        assert!(match test_segment.subsegment_from_endpoint(2) { Err(rail_segment::Error::EndpointError(2))=>true, _=>false, },
                 "Endpoint 2 points to an internal segment.");
     }