380 lines
17 KiB
Rust
380 lines
17 KiB
Rust
|
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.");
|
||
|
|
}
|
||
|
|
}
|