|
|
|
@ -1,34 +1,6 @@
|
|
|
|
#[cfg(test)]
|
|
|
|
#[cfg(test)]
|
|
|
|
mod tests {
|
|
|
|
mod tests {
|
|
|
|
use dubins_path::*;
|
|
|
|
use dubins_path::*;
|
|
|
|
use euclid::approxeq::ApproxEq;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
fn circle_in_error_margin(
|
|
|
|
|
|
|
|
result: CircleVector,
|
|
|
|
|
|
|
|
expected_result: CircleVector,
|
|
|
|
|
|
|
|
) -> Result<(), ()> {
|
|
|
|
|
|
|
|
if !ApproxEq::approx_eq(&result.center, &expected_result.center)
|
|
|
|
|
|
|
|
|| !ApproxEq::approx_eq(&result.radius, &expected_result.radius)
|
|
|
|
|
|
|
|
|| !(ApproxEq::approx_eq(&result.angle, &expected_result.angle)
|
|
|
|
|
|
|
|
|| ApproxEq::approx_eq(&result.angle.signed(), &expected_result.angle.signed()))
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
Err(())
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
|
|
|
Ok(())
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
fn vector_in_error_margin(result: Vector, expected_result: Vector) -> Result<(), ()> {
|
|
|
|
|
|
|
|
if !ApproxEq::approx_eq(&result.origin, &expected_result.origin)
|
|
|
|
|
|
|
|
|| !ApproxEq::approx_eq(&result.magnitude, &expected_result.magnitude)
|
|
|
|
|
|
|
|
|| !(ApproxEq::approx_eq(&result.angle, &expected_result.angle)
|
|
|
|
|
|
|
|
|| ApproxEq::approx_eq(&result.angle.signed(), &expected_result.angle.signed()))
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
Err(())
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
|
|
|
Ok(())
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#[test]
|
|
|
|
#[test]
|
|
|
|
#[should_panic]
|
|
|
|
#[should_panic]
|
|
|
|
@ -83,9 +55,9 @@ mod tests {
|
|
|
|
|
|
|
|
|
|
|
|
let result_rsr = RouteCSC::rsr(test_point).unwrap();
|
|
|
|
let result_rsr = RouteCSC::rsr(test_point).unwrap();
|
|
|
|
|
|
|
|
|
|
|
|
circle_in_error_margin(result_rsr.start, expected_result_rsr.start).unwrap();
|
|
|
|
assert!(result_rsr.start.approx_eq(expected_result_rsr.start));
|
|
|
|
vector_in_error_margin(result_rsr.tangent, expected_result_rsr.tangent).unwrap();
|
|
|
|
assert!(result_rsr.tangent.approx_eq(expected_result_rsr.tangent));
|
|
|
|
circle_in_error_margin(result_rsr.end, expected_result_rsr.end).unwrap();
|
|
|
|
assert!(result_rsr.end.approx_eq(expected_result_rsr.end));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
@ -118,9 +90,9 @@ mod tests {
|
|
|
|
|
|
|
|
|
|
|
|
let result_rsl = RouteCSC::rsl(test_point).unwrap();
|
|
|
|
let result_rsl = RouteCSC::rsl(test_point).unwrap();
|
|
|
|
|
|
|
|
|
|
|
|
circle_in_error_margin(result_rsl.start, expected_result_rsl.start).unwrap();
|
|
|
|
assert!(result_rsl.start.approx_eq(expected_result_rsl.start));
|
|
|
|
vector_in_error_margin(result_rsl.tangent, expected_result_rsl.tangent).unwrap();
|
|
|
|
assert!(result_rsl.tangent.approx_eq(expected_result_rsl.tangent));
|
|
|
|
circle_in_error_margin(result_rsl.end, expected_result_rsl.end).unwrap();
|
|
|
|
assert!(result_rsl.end.approx_eq(expected_result_rsl.end));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
@ -153,9 +125,9 @@ mod tests {
|
|
|
|
|
|
|
|
|
|
|
|
let result_lsl = RouteCSC::lsl(test_point).unwrap();
|
|
|
|
let result_lsl = RouteCSC::lsl(test_point).unwrap();
|
|
|
|
|
|
|
|
|
|
|
|
circle_in_error_margin(result_lsl.start, expected_result_lsl.start).unwrap();
|
|
|
|
assert!(result_lsl.start.approx_eq(expected_result_lsl.start));
|
|
|
|
vector_in_error_margin(result_lsl.tangent, expected_result_lsl.tangent).unwrap();
|
|
|
|
assert!(result_lsl.tangent.approx_eq(expected_result_lsl.tangent));
|
|
|
|
circle_in_error_margin(result_lsl.end, expected_result_lsl.end).unwrap();
|
|
|
|
assert!(result_lsl.end.approx_eq(expected_result_lsl.end));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
@ -188,9 +160,9 @@ mod tests {
|
|
|
|
|
|
|
|
|
|
|
|
let result_lsr = RouteCSC::lsr(test_point).unwrap();
|
|
|
|
let result_lsr = RouteCSC::lsr(test_point).unwrap();
|
|
|
|
|
|
|
|
|
|
|
|
circle_in_error_margin(result_lsr.start, expected_result_lsr.start).unwrap();
|
|
|
|
assert!(result_lsr.start.approx_eq(expected_result_lsr.start));
|
|
|
|
vector_in_error_margin(result_lsr.tangent, expected_result_lsr.tangent).unwrap();
|
|
|
|
assert!(result_lsr.tangent.approx_eq(expected_result_lsr.tangent));
|
|
|
|
circle_in_error_margin(result_lsr.end, expected_result_lsr.end).unwrap();
|
|
|
|
assert!(result_lsr.end.approx_eq(expected_result_lsr.end));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|