diff --git a/src/graph_coord.rs b/src/graph_coord.rs new file mode 100644 index 0000000..b3da2db --- /dev/null +++ b/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, + } + } +} diff --git a/src/lib.rs b/src/lib.rs index 0712c68..ee8100e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,4 +1,4 @@ -//mod graph_coord; +mod graph_coord; mod rail_graph; mod rail_segment; mod rail_connector; diff --git a/src/rail_graph.rs b/src/rail_graph.rs index 1d95c85..a93ecb5 100644 --- a/src/rail_graph.rs +++ b/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 { + 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 { + 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, 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."); + } } diff --git a/src/rail_segment.rs b/src/rail_segment.rs index bd793e7..1531a95 100644 --- a/src/rail_segment.rs +++ b/src/rail_segment.rs @@ -89,10 +89,17 @@ pub trait RailSegment { */ fn sample_up_vector(&self, index: usize, offset: f32) -> Result; + /** 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; + fn subsegment_from_endpoint(&self, endpoint: usize) -> Result; + + /** Get the segment index associated with an endpoint. */ + fn endpoint_from_subsegment(&self, index: usize, is_upper: bool) -> Result; /** Get the location of an endpoint. */ fn get_endpoint_transform(&self, endpoint: usize) -> Result; diff --git a/src/simple_segment.rs b/src/simple_segment.rs index e3777a8..6157b0f 100644 --- a/src/simple_segment.rs +++ b/src/simple_segment.rs @@ -116,7 +116,11 @@ impl RailSegment for SimpleSegment Result { + fn has_subsegment(&self, index: usize) -> bool { + index == 0 + } + + fn subsegment_from_endpoint(&self, endpoint: usize) -> Result { if endpoint > 1 { return Err(rail_segment::Error::EndpointError(endpoint)); } else { @@ -124,6 +128,18 @@ impl RailSegment for SimpleSegment Result { + 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 { 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."); }