pub mod orthogonal;
pub mod scratch;
pub mod smooth;
use alloc::vec::Vec;
pub use orthogonal::OrthogonalPolyline;
pub use smooth::route_smooth;
use crate::error::PathPlanError;
use crate::geom::clearance::Clearance;
use crate::geom::{BoundingBox, FreeSpace, Point2D, Polygon};
use crate::spline::CubicBezierSegment;
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PortConstraint {
pub point: Point2D,
pub direction: Option<Point2D>,
}
impl PortConstraint {
pub fn free(point: Point2D) -> Self {
Self { point, direction: None }
}
pub fn directed(point: Point2D, direction: Point2D) -> Self {
Self { point, direction: Some(direction) }
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Config {
pub margin: f64,
pub tangent_bias: f64,
pub max_repair_iters: u32,
pub repair_dampening: f64,
pub max_arc_segments: u32,
}
impl Default for Config {
fn default() -> Self {
Self {
margin: 0.0,
tangent_bias: 1.0,
max_repair_iters: 16,
repair_dampening: 0.5,
max_arc_segments: 64,
}
}
}
impl Config {
pub fn validate(&self) -> Result<(), PathPlanError> {
if !self.margin.is_finite() || self.margin < 0.0 {
return Err(PathPlanError::InvalidConfig("margin must be finite and >= 0"));
}
if !self.tangent_bias.is_finite() || self.tangent_bias < 0.0 {
return Err(PathPlanError::InvalidConfig("tangent_bias must be finite and >= 0"));
}
if self.max_repair_iters == 0 {
return Err(PathPlanError::InvalidConfig("max_repair_iters must be > 0"));
}
if !(self.repair_dampening > 0.0 && self.repair_dampening < 1.0) {
return Err(PathPlanError::InvalidConfig("repair_dampening must be in (0, 1)"));
}
if self.max_arc_segments == 0 {
return Err(PathPlanError::InvalidConfig("max_arc_segments must be > 0"));
}
Ok(())
}
}
#[derive(Clone, Debug)]
pub struct RouteRequest {
pub workspace: BoundingBox,
pub start: PortConstraint,
pub goal: PortConstraint,
pub obstacles: Vec<Polygon>,
pub config: Config,
}
impl RouteRequest {
pub fn new(workspace: BoundingBox, start: Point2D, goal: Point2D) -> Self {
Self {
workspace,
start: PortConstraint::free(start),
goal: PortConstraint::free(goal),
obstacles: Vec::new(),
config: Config::default(),
}
}
pub fn free_space(&self) -> Result<FreeSpace, PathPlanError> {
FreeSpace::new(self.workspace, self.obstacles.clone())
}
pub fn clearance(&self) -> Clearance<'_> {
Clearance::new(&self.obstacles, self.workspace, self.config.margin)
}
pub fn validate(&self) -> Result<(), PathPlanError> {
self.config.validate()?;
if self.workspace.is_degenerate() {
return Err(PathPlanError::DegenerateWorkspace);
}
for port in [&self.start, &self.goal] {
if !port.point.is_finite() {
return Err(PathPlanError::InvalidConfig("endpoint must be finite"));
}
if let Some(d) = port.direction {
if !d.is_finite() || d.is_zero() {
return Err(PathPlanError::InvalidConfig("port direction must be finite and non-zero"));
}
}
}
if self.start.point == self.goal.point {
return Err(PathPlanError::DegenerateEndpoints);
}
for port in [&self.start, &self.goal] {
if !self.workspace.contains(port.point) {
return Err(PathPlanError::EndpointOutsideWorkspace { point: port.point });
}
for obstacle in &self.obstacles {
if obstacle.contains(port.point) {
return Err(PathPlanError::EndpointInObstacle { point: port.point });
}
}
}
Ok(())
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum RouteKind {
Smooth,
Orthogonal,
}
#[derive(Clone, Debug, PartialEq)]
pub enum Route {
Smooth(Vec<CubicBezierSegment>),
Orthogonal(OrthogonalPolyline),
}
#[derive(Debug, Default)]
pub struct PathPlanner {
pub(crate) scratch: scratch::Scratch,
}
impl PathPlanner {
pub fn new() -> Self {
Self { scratch: scratch::Scratch::default() }
}
pub fn route_smooth(
&mut self,
req: &RouteRequest,
) -> Result<Vec<CubicBezierSegment>, PathPlanError> {
route_smooth(&mut self.scratch, req)
}
pub fn route_orthogonal(
&mut self,
req: &RouteRequest,
) -> Result<OrthogonalPolyline, PathPlanError> {
orthogonal::route_orthogonal(&mut self.scratch, req)
}
pub fn route(&mut self, req: &RouteRequest, kind: RouteKind) -> Result<Route, PathPlanError> {
match kind {
RouteKind::Smooth => self.route_smooth(req).map(Route::Smooth),
RouteKind::Orthogonal => self.route_orthogonal(req).map(Route::Orthogonal),
}
}
}