dubins_path/tests/unit_test.rs

#[cfg(test)]
mod tests {
    use dubins_path::*;
    use float_cmp::approx_eq;

    fn circle_in_error_margin(
        result: CircleVector,
        expected_result: CircleVector,
    ) -> Result<(), ()> {
        assert!(approx_eq!(f64, result.center.x, expected_result.center.x));
        assert!(approx_eq!(f64, result.center.x, expected_result.center.x));
        assert!(approx_eq!(f64, result.center.y, expected_result.center.y));
        assert!(approx_eq!(f64, result.center.y, expected_result.center.y));
        assert!(approx_eq!(f64, result.radius, expected_result.radius));
        assert!(approx_eq!(f64, result.radius, expected_result.radius));
        assert!(approx_eq!(f64, result.angle.radians, expected_result.angle.radians) ||
        	    approx_eq!(f64, result.angle.signed().radians, expected_result.angle.signed().radians));
        assert!(approx_eq!(f64, result.angle.radians, expected_result.angle.radians) ||
                approx_eq!(f64, result.angle.signed().radians, expected_result.angle.signed().radians));
        Ok(())
    }

    fn vector_in_error_margin(result: Vector, expected_result: Vector) -> Result<(), ()> {
        assert!(approx_eq!(f64, result.origin.x, expected_result.origin.x));
        assert!(approx_eq!(f64, result.origin.x, expected_result.origin.x));
        assert!(approx_eq!(f64, result.origin.y, expected_result.origin.y));
        assert!(approx_eq!(f64, result.origin.y, expected_result.origin.y));
        assert!(approx_eq!(f64, result.angle.radians, expected_result.angle.radians) ||
                approx_eq!(f64, result.angle.signed().radians, expected_result.angle.signed().radians));
        assert!(approx_eq!(f64, result.angle.radians, expected_result.angle.radians) ||
        	    approx_eq!(f64, result.angle.signed().radians, expected_result.angle.signed().radians));
        assert!(approx_eq!(f64, result.magnitude, expected_result.magnitude));
        assert!(approx_eq!(f64, result.magnitude, expected_result.magnitude));
        Ok(())
    }

    #[test]
    #[should_panic]
    fn test_overlapping_circles_rsl() {
        let radius = 0.5;
        let test_point = Vector {
            origin: Point::new(radius, 0.0),
            angle: Angle::zero(),
            magnitude: radius,
        };
        RouteCSC::rsl(test_point).unwrap();
    }

    #[test]
    #[should_panic]
    fn test_overlapping_circles_lsr() {
        let radius = 0.5;
        let test_point = Vector {
            origin: Point::new(-radius, 0.0),
            angle: Angle::zero(),
            magnitude: radius,
        };
        RouteCSC::lsr(test_point).unwrap();
    }

    #[test]
    fn test_points_rsr() {
        {
            let radius = 0.5;
            let test_point = Vector {
                origin: Point::new(0.0, 10.0),
                angle: Angle::zero(),
                magnitude: radius,
            };
            let expected_result_rsr = RouteCSC {
                start: CircleVector {
                    center: Point::new(0.5, 0.0),
                    radius: radius,
                    angle: Angle::zero(),
                },
                tangent: Vector {
                    origin: Point::new(0.0, 0.0),
                    angle: Angle::frac_pi_2(),
                    magnitude: 10.0,
                },
                end: CircleVector {
                    center: Point::new(0.5, 10.0),
                    radius: radius,
                    angle: Angle::zero(),
                },
            };

            let result_rsr = RouteCSC::rsr(test_point).unwrap();

            circle_in_error_margin(result_rsr.start, expected_result_rsr.start).unwrap();
            vector_in_error_margin(result_rsr.tangent, expected_result_rsr.tangent).unwrap();
            circle_in_error_margin(result_rsr.end, expected_result_rsr.end).unwrap();
        }
    }

    #[test]
    fn test_points_rsl() {
        {
            let radius = 0.5;
            let test_point = Vector {
                origin: Point::new(0.0, 10.0),
                angle: Angle::zero(),
                magnitude: radius,
            };
            let expected_result_rsl = RouteCSC {
                start: CircleVector {
                    center: Point::new(0.5, 0.0),
                    radius: radius,
                    angle: Angle::zero(),
                },
                tangent: Vector {
                    origin: Point::new(0.0, 0.0),
                    angle: Angle::frac_pi_2(),
                    magnitude: 10.0,
                },
                end: CircleVector {
                    center: Point::new(-0.5, 10.0),
                    radius: radius,
                    angle: Angle::zero(),
                },
            };

            let result_rsl = RouteCSC::rsl(test_point).unwrap();

            circle_in_error_margin(result_rsl.start, expected_result_rsl.start).unwrap();
            vector_in_error_margin(result_rsl.tangent, expected_result_rsl.tangent).unwrap();
            circle_in_error_margin(result_rsl.end, expected_result_rsl.end).unwrap();
        }
    }

    #[test]
    fn test_points_lsl() {
        {
            let radius = 0.5;
            let test_point = Vector {
                origin: Point::new(0.0, 10.0),
                angle: Angle::zero(),
                magnitude: radius,
            };
            let expected_result_lsl = RouteCSC {
                start: CircleVector {
                    center: Point::new(-0.5, 0.0),
                    radius: radius,
                    angle: Angle::zero(),
                },
                tangent: Vector {
                    origin: Point::new(0.0, 0.0),
                    angle: Angle::frac_pi_2(),
                    magnitude: 10.0,
                },
                end: CircleVector {
                    center: Point::new(-0.5, 10.0),
                    radius: radius,
                    angle: Angle::zero(),
                },
            };

            let result_lsl = RouteCSC::lsl(test_point).unwrap();

            circle_in_error_margin(result_lsl.start, expected_result_lsl.start).unwrap();
            vector_in_error_margin(result_lsl.tangent, expected_result_lsl.tangent).unwrap();
            circle_in_error_margin(result_lsl.end, expected_result_lsl.end).unwrap();
        }
    }

    #[test]
    fn test_point_lsr() {
        {
            let radius = 0.5;
            let test_point = Vector {
                origin: Point::new(0.0, 10.0),
                angle: Angle::zero(),
                magnitude: radius,
            };
            let expected_result_lsr = RouteCSC {
                start: CircleVector {
                    center: Point::new(-0.5, 0.0),
                    radius: radius,
                    angle: Angle::zero(),
                },
                tangent: Vector {
                    origin: Point::new(0.0, 0.0),
                    angle: Angle::frac_pi_2(),
                    magnitude: 10.0,
                },
                end: CircleVector {
                    center: Point::new(0.5, 10.0),
                    radius: radius,
                    angle: Angle::zero(),
                },
            };

            let result_lsr = RouteCSC::lsr(test_point).unwrap();

            circle_in_error_margin(result_lsr.start, expected_result_lsr.start).unwrap();
            vector_in_error_margin(result_lsr.tangent, expected_result_lsr.tangent).unwrap();
            circle_in_error_margin(result_lsr.end, expected_result_lsr.end).unwrap();
        }
    }
}