use std::sync::Arc;
use axioval_ir::{Evidence, ObjectId};
use thiserror::Error;
use crate::{ConvexPlanRegion, ElevationInterval, MetricDirection};
#[derive(Clone, Debug, Error, PartialEq, Eq)]
pub enum WalkingSurfaceError {
#[error("no geometry for `{0}`")]
UnknownObject(ObjectId),
#[error("walking surface unavailable: {0}")]
Unavailable(String),
#[error("walking surface unsupported: {0}")]
Unsupported(String),
#[error("walking surface is inexact: {0}")]
InexactGeometry(String),
#[error("walking surface measurement is invalid")]
InvalidMeasurement,
#[error("walking surface evidence does not match its exactness")]
InexactEvidence,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct MeasuredInterval {
lower: f64,
upper: f64,
}
impl MeasuredInterval {
pub fn try_new(lower: f64, upper: f64) -> Result<Self, WalkingSurfaceError> {
if !lower.is_finite() || !upper.is_finite() || lower > upper {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(Self { lower, upper })
}
#[must_use]
pub fn lower(&self) -> f64 {
self.lower
}
#[must_use]
pub fn upper(&self) -> f64 {
self.upper
}
#[must_use]
#[allow(clippy::float_cmp)]
pub fn is_point(&self) -> bool {
self.lower == self.upper
}
}
fn between(a: ElevationInterval, b: ElevationInterval) -> MeasuredInterval {
let low = subtract_down(a.lower_metres(), b.upper_metres());
let high = subtract_up(a.upper_metres(), b.lower_metres());
MeasuredInterval {
lower: low,
upper: high.max(low),
}
}
fn subtract_down(x: f64, y: f64) -> f64 {
let (rounded, error) = two_difference(x, y);
if error < 0.0 {
rounded.next_down()
} else {
rounded
}
}
fn subtract_up(x: f64, y: f64) -> f64 {
let (rounded, error) = two_difference(x, y);
if error > 0.0 {
rounded.next_up()
} else {
rounded
}
}
fn two_difference(x: f64, y: f64) -> (f64, f64) {
let rounded = x - y;
let back = rounded - x;
let error = (x - (rounded - back)) + (-y - back);
(rounded, error)
}
fn divide(x: f64, y: f64, up: bool) -> f64 {
let rounded = x / y;
let residual = rounded.mul_add(y, -x);
match (up, residual) {
(true, residual) if residual < 0.0 => rounded.next_up(),
(false, residual) if residual > 0.0 => rounded.next_down(),
_ => rounded,
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PlanSegment {
from: [f64; 2],
to: [f64; 2],
radius: f64,
}
impl PlanSegment {
pub fn try_new(from: [f64; 2], to: [f64; 2], radius: f64) -> Result<Self, WalkingSurfaceError> {
let finite = from.iter().chain(&to).all(|value| value.is_finite());
#[allow(clippy::float_cmp)]
if !finite || from == to || !radius.is_finite() || radius < 0.0 {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(Self { from, to, radius })
}
#[must_use]
pub fn from(&self) -> [f64; 2] {
self.from
}
#[must_use]
pub fn to(&self) -> [f64; 2] {
self.to
}
#[must_use]
pub fn radius(&self) -> f64 {
self.radius
}
#[must_use]
pub fn angle_to(&self, other: &PlanSegment) -> Option<MeasuredInterval> {
let turn = |segment: &PlanSegment| -> Option<([f64; 2], f64)> {
let vector = [
segment.to[0] - segment.from[0],
segment.to[1] - segment.from[1],
];
let magnitude = segment
.from
.iter()
.chain(&segment.to)
.fold(0.0_f64, |most, value| most.max(value.abs()));
let radius = 4.0f64.mul_add(f64::EPSILON * magnitude, segment.radius);
let length = vector[0].hypot(vector[1]) * 4.0f64.mul_add(-f64::EPSILON, 1.0);
if length <= 2.0 * radius {
return None;
}
Some((vector, (2.0 * radius / length).asin()))
};
let (u, first) = turn(self)?;
let (v, second) = turn(other)?;
let cross = u[0].mul_add(v[1], -(u[1] * v[0]));
let dot = u[0].mul_add(v[0], u[1] * v[1]);
let angle = cross.abs().atan2(dot.abs());
let spread = first + second + 16.0 * f64::EPSILON;
let lower = (angle - spread).max(0.0);
let upper = (angle + spread).min(std::f64::consts::FRAC_PI_2).max(lower);
MeasuredInterval::try_new(lower, upper).ok()
}
fn is_exact(&self) -> bool {
self.radius == 0.0
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum RiserClosure {
Closed,
Open,
NotMeasured,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Tread {
elevation: ElevationInterval,
front: ElevationInterval,
back: ElevationInterval,
sides: Option<Sides>,
nosing: Option<PlanSegment>,
riser_below: RiserClosure,
}
type Sides = (ElevationInterval, ElevationInterval);
#[must_use]
pub fn across(direction: MetricDirection) -> MetricDirection {
let [x, y, _] = direction.components();
MetricDirection::try_new([-y, x, 0.0]).unwrap_or(direction)
}
fn sides(left: ElevationInterval, right: ElevationInterval) -> Result<Sides, WalkingSurfaceError> {
if left.lower_metres() > right.lower_metres() || left.upper_metres() > right.upper_metres() {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok((left, right))
}
fn sides_exact(sides: Option<Sides>) -> bool {
sides.is_none_or(|(left, right)| left.is_exact() && right.is_exact())
}
fn least(widths: impl Iterator<Item = MeasuredInterval>) -> Option<MeasuredInterval> {
widths.reduce(|least, width| MeasuredInterval {
lower: least.lower.min(width.lower),
upper: least.upper.min(width.upper),
})
}
impl Tread {
pub fn try_new(
elevation: ElevationInterval,
front: ElevationInterval,
back: ElevationInterval,
) -> Result<Self, WalkingSurfaceError> {
if front.lower_metres() > back.lower_metres() || front.upper_metres() > back.upper_metres()
{
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(Self {
elevation,
front,
back,
sides: None,
nosing: None,
riser_below: RiserClosure::NotMeasured,
})
}
pub fn with_sides(
mut self,
left: ElevationInterval,
right: ElevationInterval,
) -> Result<Self, WalkingSurfaceError> {
self.sides = Some(sides(left, right)?);
Ok(self)
}
#[must_use]
pub fn with_nosing(mut self, nosing: PlanSegment) -> Self {
self.nosing = Some(nosing);
self
}
#[must_use]
pub fn with_riser_below(mut self, riser: RiserClosure) -> Self {
self.riser_below = riser;
self
}
#[must_use]
pub fn sides(&self) -> Option<(ElevationInterval, ElevationInterval)> {
self.sides
}
#[must_use]
pub fn width(&self) -> Option<MeasuredInterval> {
self.sides.map(|(left, right)| between(right, left))
}
#[must_use]
pub fn elevation(&self) -> ElevationInterval {
self.elevation
}
#[must_use]
pub fn front(&self) -> ElevationInterval {
self.front
}
#[must_use]
pub fn back(&self) -> ElevationInterval {
self.back
}
#[must_use]
pub fn nosing(&self) -> Option<PlanSegment> {
self.nosing
}
#[must_use]
pub fn riser_below(&self) -> RiserClosure {
self.riser_below
}
#[must_use]
pub fn depth(&self) -> MeasuredInterval {
between(self.back, self.front)
}
#[must_use]
pub fn is_exact(&self) -> bool {
self.elevation.is_exact()
&& self.front.is_exact()
&& self.back.is_exact()
&& sides_exact(self.sides)
&& self.nosing.is_none_or(|nosing| nosing.is_exact())
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum WalkingLinePlacement {
Centre,
FromInnerSide(f64),
}
#[derive(Clone, Debug, PartialEq)]
pub struct TreadFlightRequest {
object: ObjectId,
walking_line: WalkingLinePlacement,
}
impl TreadFlightRequest {
#[must_use]
pub fn new(object: ObjectId) -> Self {
Self {
object,
walking_line: WalkingLinePlacement::Centre,
}
}
pub fn from_inner_side(object: ObjectId, offset: f64) -> Result<Self, WalkingSurfaceError> {
if !offset.is_finite() || offset <= 0.0 {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(Self {
object,
walking_line: WalkingLinePlacement::FromInnerSide(offset),
})
}
#[must_use]
pub fn object(&self) -> &ObjectId {
&self.object
}
#[must_use]
pub fn walking_line(&self) -> WalkingLinePlacement {
self.walking_line
}
}
#[derive(Clone, Debug, PartialEq)]
pub enum WalkingLine {
Straight(MetricDirection),
Turning(Vec<[f64; 2]>),
}
impl WalkingLine {
#[must_use]
pub fn is_turning(&self) -> bool {
matches!(self, Self::Turning(_))
}
#[allow(clippy::float_cmp)]
fn is_valid(&self) -> bool {
match self {
Self::Straight(direction) => direction.components()[2] == 0.0,
Self::Turning(vertices) => {
vertices.len() >= 2
&& vertices.iter().flatten().all(|value| value.is_finite())
&& vertices.windows(2).all(|pair| pair[0] != pair[1])
}
}
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct TreadFlight {
request: TreadFlightRequest,
walking_line: WalkingLine,
base: ElevationInterval,
top: ElevationInterval,
treads: Vec<Tread>,
ends_in_riser: bool,
final_riser: RiserClosure,
evidence: Evidence,
}
impl TreadFlight {
pub fn try_new(
request: TreadFlightRequest,
walking_line: WalkingLine,
base: ElevationInterval,
top: ElevationInterval,
treads: Vec<Tread>,
evidence: Evidence,
) -> Result<Self, WalkingSurfaceError> {
if !walking_line.is_valid() {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
let (Some(first), Some(last)) = (treads.first(), treads.last()) else {
return Err(WalkingSurfaceError::InvalidMeasurement);
};
if base.upper_metres() >= first.elevation.lower_metres() {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
for pair in treads.windows(2) {
if pair[0].elevation.upper_metres() >= pair[1].elevation.lower_metres()
|| pair[0].front.upper_metres() >= pair[1].front.lower_metres()
{
return Err(WalkingSurfaceError::InvalidMeasurement);
}
}
let ends_in_riser = if top.lower_metres() > last.elevation.upper_metres() {
true
} else if top.upper_metres() <= last.elevation.upper_metres()
&& top.upper_metres() >= last.elevation.lower_metres()
{
false
} else {
return Err(WalkingSurfaceError::InvalidMeasurement);
};
let exact = base.is_exact() && top.is_exact() && treads.iter().all(Tread::is_exact);
if evidence.exact != exact || evidence.locator.trim().is_empty() {
return Err(WalkingSurfaceError::InexactEvidence);
}
Ok(Self {
request,
walking_line,
base,
top,
treads,
ends_in_riser,
final_riser: RiserClosure::NotMeasured,
evidence,
})
}
#[must_use]
pub fn with_final_riser(mut self, riser: RiserClosure) -> Self {
self.final_riser = riser;
self
}
#[must_use]
pub fn request(&self) -> &TreadFlightRequest {
&self.request
}
#[must_use]
pub fn object(&self) -> &ObjectId {
&self.request.object
}
#[must_use]
pub fn walking_line(&self) -> &WalkingLine {
&self.walking_line
}
#[must_use]
pub fn base(&self) -> ElevationInterval {
self.base
}
#[must_use]
pub fn top(&self) -> ElevationInterval {
self.top
}
#[must_use]
pub fn treads(&self) -> &[Tread] {
&self.treads
}
#[must_use]
pub fn ends_in_riser(&self) -> bool {
self.ends_in_riser
}
#[must_use]
pub fn risers(&self) -> Vec<MeasuredInterval> {
let mut levels = vec![self.base];
levels.extend(self.treads.iter().map(|tread| tread.elevation));
if self.ends_in_riser {
levels.push(self.top);
}
levels
.windows(2)
.map(|pair| between(pair[1], pair[0]))
.collect()
}
#[must_use]
pub fn riser_closures(&self) -> Vec<RiserClosure> {
let mut closures: Vec<RiserClosure> =
self.treads.iter().map(|tread| tread.riser_below).collect();
if self.ends_in_riser {
closures.push(self.final_riser);
}
closures
}
#[must_use]
pub fn goings(&self) -> Vec<MeasuredInterval> {
self.treads
.windows(2)
.map(|pair| between(pair[1].front, pair[0].front))
.collect()
}
#[must_use]
pub fn nosings(&self) -> Vec<MeasuredInterval> {
self.treads
.windows(2)
.map(|pair| between(pair[0].back, pair[1].front))
.collect()
}
#[must_use]
pub fn winder_angles(&self) -> Vec<Option<MeasuredInterval>> {
self.treads
.windows(2)
.map(|pair| match (pair[0].nosing, pair[1].nosing) {
(Some(lower), Some(upper)) => lower.angle_to(&upper),
_ => None,
})
.collect()
}
#[must_use]
pub fn rise(&self) -> MeasuredInterval {
let summit = if self.ends_in_riser {
self.top
} else {
self.treads.last().map_or(self.top, |tread| tread.elevation)
};
between(summit, self.base)
}
#[must_use]
pub fn width(&self) -> Option<MeasuredInterval> {
let widths: Option<Vec<MeasuredInterval>> = self.treads.iter().map(Tread::width).collect();
least(widths?.into_iter())
}
#[must_use]
pub fn is_exact(&self) -> bool {
self.evidence.exact
}
#[must_use]
pub fn evidence(&self) -> &Evidence {
&self.evidence
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct SlopedRun {
direction: MetricDirection,
bottom: ElevationInterval,
top: ElevationInterval,
start: ElevationInterval,
end: ElevationInterval,
sides: Option<Sides>,
}
impl SlopedRun {
pub fn try_new(
direction: MetricDirection,
bottom: ElevationInterval,
top: ElevationInterval,
start: ElevationInterval,
end: ElevationInterval,
) -> Result<Self, WalkingSurfaceError> {
#[allow(clippy::float_cmp)]
if direction.components()[2] != 0.0
|| bottom.upper_metres() >= top.lower_metres()
|| start.upper_metres() >= end.lower_metres()
{
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(Self {
direction,
bottom,
top,
start,
end,
sides: None,
})
}
pub fn with_sides(
mut self,
left: ElevationInterval,
right: ElevationInterval,
) -> Result<Self, WalkingSurfaceError> {
self.sides = Some(sides(left, right)?);
Ok(self)
}
#[must_use]
pub fn sides(&self) -> Option<(ElevationInterval, ElevationInterval)> {
self.sides
}
#[must_use]
pub fn width(&self) -> Option<MeasuredInterval> {
self.sides.map(|(left, right)| between(right, left))
}
#[must_use]
pub fn direction(&self) -> MetricDirection {
self.direction
}
#[must_use]
pub fn bottom(&self) -> ElevationInterval {
self.bottom
}
#[must_use]
pub fn top(&self) -> ElevationInterval {
self.top
}
#[must_use]
pub fn start(&self) -> ElevationInterval {
self.start
}
#[must_use]
pub fn end(&self) -> ElevationInterval {
self.end
}
#[must_use]
pub fn rise(&self) -> MeasuredInterval {
between(self.top, self.bottom)
}
#[must_use]
pub fn length(&self) -> MeasuredInterval {
between(self.end, self.start)
}
#[must_use]
pub fn slope(&self) -> MeasuredInterval {
let (rise, length) = (self.rise(), self.length());
let lower = divide(rise.lower.max(0.0), length.upper, false);
let upper = divide(rise.upper, length.lower, true);
MeasuredInterval {
lower,
upper: upper.max(lower),
}
}
#[must_use]
pub fn is_exact(&self) -> bool {
self.bottom.is_exact()
&& self.top.is_exact()
&& self.start.is_exact()
&& self.end.is_exact()
&& sides_exact(self.sides)
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct SlopedSurface {
object: ObjectId,
runs: Vec<SlopedRun>,
evidence: Evidence,
}
impl SlopedSurface {
pub fn try_new(
object: ObjectId,
runs: Vec<SlopedRun>,
evidence: Evidence,
) -> Result<Self, WalkingSurfaceError> {
if runs.is_empty() {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
let exact = runs.iter().all(SlopedRun::is_exact);
if evidence.exact != exact || evidence.locator.trim().is_empty() {
return Err(WalkingSurfaceError::InexactEvidence);
}
Ok(Self {
object,
runs,
evidence,
})
}
#[must_use]
pub fn object(&self) -> &ObjectId {
&self.object
}
#[must_use]
pub fn runs(&self) -> &[SlopedRun] {
&self.runs
}
#[must_use]
pub fn is_exact(&self) -> bool {
self.evidence.exact
}
#[must_use]
pub fn evidence(&self) -> &Evidence {
&self.evidence
}
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct HeadroomRequest {
subject: ObjectId,
obstacles: Vec<ObjectId>,
}
impl HeadroomRequest {
#[must_use]
pub fn new(subject: ObjectId, obstacles: impl IntoIterator<Item = ObjectId>) -> Self {
let mut obstacles: Vec<ObjectId> = obstacles
.into_iter()
.filter(|obstacle| *obstacle != subject)
.collect();
obstacles.sort();
obstacles.dedup();
Self { subject, obstacles }
}
#[must_use]
pub fn subject(&self) -> &ObjectId {
&self.subject
}
#[must_use]
pub fn obstacles(&self) -> &[ObjectId] {
&self.obstacles
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct Headroom {
request: HeadroomRequest,
clearance: Option<MeasuredInterval>,
governing: Vec<ObjectId>,
evidence: Evidence,
}
impl Headroom {
pub fn try_new(
request: HeadroomRequest,
clearance: Option<MeasuredInterval>,
governing: Vec<ObjectId>,
evidence: Evidence,
) -> Result<Self, WalkingSurfaceError> {
let governing = governed(&request.obstacles, clearance, governing, &evidence)?;
Ok(Self {
request,
clearance,
governing,
evidence,
})
}
#[must_use]
pub fn request(&self) -> &HeadroomRequest {
&self.request
}
#[must_use]
pub fn clearance(&self) -> Option<MeasuredInterval> {
self.clearance
}
#[must_use]
pub fn governing(&self) -> &[ObjectId] {
&self.governing
}
#[must_use]
pub fn evidence(&self) -> &Evidence {
&self.evidence
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub enum WalkingEnd {
FlightBottom,
FlightTop,
RunBottom(usize),
RunTop(usize),
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct LandingRequest {
subject: ObjectId,
end: WalkingEnd,
candidates: Vec<ObjectId>,
}
impl LandingRequest {
#[must_use]
pub fn new(
subject: ObjectId,
end: WalkingEnd,
candidates: impl IntoIterator<Item = ObjectId>,
) -> Self {
let mut candidates: Vec<ObjectId> = candidates
.into_iter()
.filter(|candidate| *candidate != subject)
.collect();
candidates.sort();
candidates.dedup();
Self {
subject,
end,
candidates,
}
}
#[must_use]
pub fn subject(&self) -> &ObjectId {
&self.subject
}
#[must_use]
pub fn end(&self) -> WalkingEnd {
self.end
}
#[must_use]
pub fn candidates(&self) -> &[ObjectId] {
&self.candidates
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct LandingExtent {
far: ElevationInterval,
left: ElevationInterval,
right: ElevationInterval,
}
impl LandingExtent {
pub fn try_new(
far: ElevationInterval,
left: ElevationInterval,
right: ElevationInterval,
) -> Result<Self, WalkingSurfaceError> {
let (left, right) = sides(left, right)?;
Ok(Self { far, left, right })
}
#[must_use]
pub fn far(&self) -> ElevationInterval {
self.far
}
#[must_use]
pub fn sides(&self) -> (ElevationInterval, ElevationInterval) {
(self.left, self.right)
}
fn is_exact(&self) -> bool {
self.far.is_exact() && sides_exact(Some((self.left, self.right)))
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct Landing {
carrier: ObjectId,
extent: Option<LandingExtent>,
}
impl Landing {
#[must_use]
pub fn new(carrier: ObjectId, extent: Option<LandingExtent>) -> Self {
Self { carrier, extent }
}
#[must_use]
pub fn carrier(&self) -> &ObjectId {
&self.carrier
}
#[must_use]
pub fn extent(&self) -> Option<LandingExtent> {
self.extent
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct LandingEvidence {
request: LandingRequest,
direction: MetricDirection,
edge: ElevationInterval,
landing: Option<Landing>,
evidence: Evidence,
}
impl LandingEvidence {
pub fn try_new(
request: LandingRequest,
direction: MetricDirection,
edge: ElevationInterval,
landing: Option<Landing>,
evidence: Evidence,
) -> Result<Self, WalkingSurfaceError> {
#[allow(clippy::float_cmp)]
if direction.components()[2] != 0.0 {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
if let Some(landing) = &landing {
let named = landing.carrier == request.subject
|| request.candidates.binary_search(&landing.carrier).is_ok();
let beyond = landing
.extent
.is_none_or(|extent| extent.far.lower_metres() > edge.upper_metres());
if !named || !beyond {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
}
let exact = edge.is_exact()
&& landing
.as_ref()
.and_then(|landing| landing.extent)
.is_none_or(|extent| extent.is_exact());
if evidence.exact != exact || evidence.locator.trim().is_empty() {
return Err(WalkingSurfaceError::InexactEvidence);
}
Ok(Self {
request,
direction,
edge,
landing,
evidence,
})
}
#[must_use]
pub fn request(&self) -> &LandingRequest {
&self.request
}
#[must_use]
pub fn direction(&self) -> MetricDirection {
self.direction
}
#[must_use]
pub fn edge(&self) -> ElevationInterval {
self.edge
}
#[must_use]
pub fn landing(&self) -> Option<&Landing> {
self.landing.as_ref()
}
#[must_use]
pub fn depth(&self) -> Option<MeasuredInterval> {
let extent = self.landing.as_ref()?.extent?;
Some(between(extent.far, self.edge))
}
#[must_use]
pub fn width(&self) -> Option<MeasuredInterval> {
let extent = self.landing.as_ref()?.extent?;
Some(between(extent.right, extent.left))
}
#[must_use]
pub fn evidence(&self) -> &Evidence {
&self.evidence
}
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct ClearanceBelowRequest {
subject: ObjectId,
spaces: Vec<ObjectId>,
}
impl ClearanceBelowRequest {
#[must_use]
pub fn new(subject: ObjectId, spaces: impl IntoIterator<Item = ObjectId>) -> Self {
let mut spaces: Vec<ObjectId> = spaces
.into_iter()
.filter(|space| *space != subject)
.collect();
spaces.sort();
spaces.dedup();
Self { subject, spaces }
}
#[must_use]
pub fn subject(&self) -> &ObjectId {
&self.subject
}
#[must_use]
pub fn spaces(&self) -> &[ObjectId] {
&self.spaces
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct ClearanceBelow {
request: ClearanceBelowRequest,
clearance: Option<MeasuredInterval>,
governing: Vec<ObjectId>,
evidence: Evidence,
}
impl ClearanceBelow {
pub fn try_new(
request: ClearanceBelowRequest,
clearance: Option<MeasuredInterval>,
governing: Vec<ObjectId>,
evidence: Evidence,
) -> Result<Self, WalkingSurfaceError> {
let governing = governed(&request.spaces, clearance, governing, &evidence)?;
Ok(Self {
request,
clearance,
governing,
evidence,
})
}
#[must_use]
pub fn request(&self) -> &ClearanceBelowRequest {
&self.request
}
#[must_use]
pub fn clearance(&self) -> Option<MeasuredInterval> {
self.clearance
}
#[must_use]
pub fn governing(&self) -> &[ObjectId] {
&self.governing
}
#[must_use]
pub fn evidence(&self) -> &Evidence {
&self.evidence
}
}
fn governed(
named: &[ObjectId],
clearance: Option<MeasuredInterval>,
mut governing: Vec<ObjectId>,
evidence: &Evidence,
) -> Result<Vec<ObjectId>, WalkingSurfaceError> {
governing.sort();
governing.dedup();
let requested = governing
.iter()
.all(|object| named.binary_search(object).is_ok());
if !requested || clearance.is_some() == governing.is_empty() {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
if clearance.is_some_and(|clearance| clearance.lower < 0.0) {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
if evidence.locator.trim().is_empty()
|| (evidence.exact && clearance.is_some_and(|clearance| !clearance.is_point()))
{
return Err(WalkingSurfaceError::InexactEvidence);
}
Ok(governing)
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub enum WalkingStretch {
Flight,
Run(usize),
}
#[derive(Clone, Debug, PartialEq)]
pub struct HandrailRequest {
subject: ObjectId,
stretch: WalkingStretch,
rails: Vec<ObjectId>,
reach: f64,
above: f64,
extension: f64,
}
impl HandrailRequest {
pub fn try_new(
subject: ObjectId,
stretch: WalkingStretch,
rails: impl IntoIterator<Item = ObjectId>,
(reach, above): (f64, f64),
extension: f64,
) -> Result<Self, WalkingSurfaceError> {
if [reach, above, extension]
.iter()
.any(|length| !length.is_finite() || *length < 0.0)
{
return Err(WalkingSurfaceError::InvalidMeasurement);
}
let mut rails: Vec<ObjectId> = rails.into_iter().filter(|rail| *rail != subject).collect();
rails.sort();
rails.dedup();
Ok(Self {
subject,
stretch,
rails,
reach,
above,
extension,
})
}
#[must_use]
pub fn subject(&self) -> &ObjectId {
&self.subject
}
#[must_use]
pub fn stretch(&self) -> WalkingStretch {
self.stretch
}
#[must_use]
pub fn rails(&self) -> &[ObjectId] {
&self.rails
}
#[must_use]
pub fn reach(&self) -> f64 {
self.reach
}
#[must_use]
pub fn above(&self) -> f64 {
self.above
}
#[must_use]
pub fn extension(&self) -> f64 {
self.extension
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct ClearWidthRequest {
subject: ObjectId,
stretch: WalkingStretch,
obstacles: Vec<ObjectId>,
band: (f64, f64),
}
impl ClearWidthRequest {
pub fn try_new(
subject: ObjectId,
stretch: WalkingStretch,
obstacles: impl IntoIterator<Item = ObjectId>,
band: (f64, f64),
) -> Result<Self, WalkingSurfaceError> {
let (from, to) = band;
if !from.is_finite() || !to.is_finite() || from < 0.0 || to <= from {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
let mut obstacles: Vec<ObjectId> = obstacles
.into_iter()
.filter(|obstacle| *obstacle != subject)
.collect();
obstacles.sort();
obstacles.dedup();
Ok(Self {
subject,
stretch,
obstacles,
band,
})
}
#[must_use]
pub fn subject(&self) -> &ObjectId {
&self.subject
}
#[must_use]
pub fn stretch(&self) -> WalkingStretch {
self.stretch
}
#[must_use]
pub fn obstacles(&self) -> &[ObjectId] {
&self.obstacles
}
#[must_use]
pub fn band(&self) -> (f64, f64) {
self.band
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct ClearWidthEvidence {
request: ClearWidthRequest,
width: MeasuredInterval,
governing: Vec<ObjectId>,
evidence: Evidence,
}
impl ClearWidthEvidence {
pub fn try_new(
request: ClearWidthRequest,
width: MeasuredInterval,
mut governing: Vec<ObjectId>,
evidence: Evidence,
) -> Result<Self, WalkingSurfaceError> {
governing.sort();
governing.dedup();
if width.lower < 0.0
|| !governing
.iter()
.all(|object| request.obstacles.binary_search(object).is_ok())
{
return Err(WalkingSurfaceError::InvalidMeasurement);
}
if evidence.locator.trim().is_empty() || (evidence.exact && !width.is_point()) {
return Err(WalkingSurfaceError::InexactEvidence);
}
Ok(Self {
request,
width,
governing,
evidence,
})
}
#[must_use]
pub fn request(&self) -> &ClearWidthRequest {
&self.request
}
#[must_use]
pub fn width(&self) -> MeasuredInterval {
self.width
}
#[must_use]
pub fn governing(&self) -> &[ObjectId] {
&self.governing
}
#[must_use]
pub fn evidence(&self) -> &Evidence {
&self.evidence
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub enum RailSide {
Right,
Left,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct StretchPart {
direction: MetricDirection,
sides: (ElevationInterval, ElevationInterval),
}
impl StretchPart {
pub fn try_new(
direction: MetricDirection,
(left, right): (ElevationInterval, ElevationInterval),
) -> Result<Self, WalkingSurfaceError> {
#[allow(clippy::float_cmp)]
if direction.components()[2] != 0.0 {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(Self {
direction,
sides: sides(left, right)?,
})
}
#[must_use]
pub fn direction(&self) -> MetricDirection {
self.direction
}
#[must_use]
pub fn sides(&self) -> (ElevationInterval, ElevationInterval) {
self.sides
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct RailMeasurement {
part: usize,
start: ElevationInterval,
end: ElevationInterval,
left: ElevationInterval,
right: ElevationInterval,
lowest: MeasuredInterval,
highest: MeasuredInterval,
bottom_rise: Option<MeasuredInterval>,
top_rise: Option<MeasuredInterval>,
}
impl RailMeasurement {
pub fn try_new(
(start, end): (ElevationInterval, ElevationInterval),
(left, right): (ElevationInterval, ElevationInterval),
lowest: MeasuredInterval,
highest: MeasuredInterval,
) -> Result<Self, WalkingSurfaceError> {
let (start, end) = sides(start, end)?;
let (left, right) = sides(left, right)?;
if lowest.lower > highest.lower || lowest.upper > highest.upper {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(Self {
part: 0,
start,
end,
left,
right,
lowest,
highest,
bottom_rise: None,
top_rise: None,
})
}
#[must_use]
pub fn in_part(mut self, part: usize) -> Self {
self.part = part;
self
}
#[must_use]
pub fn part(&self) -> usize {
self.part
}
pub fn with_rises(
mut self,
bottom: Option<MeasuredInterval>,
top: Option<MeasuredInterval>,
) -> Result<Self, WalkingSurfaceError> {
if [bottom, top].iter().flatten().any(|rise| rise.lower < 0.0) {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
self.bottom_rise = bottom;
self.top_rise = top;
Ok(self)
}
#[must_use]
pub fn start(&self) -> ElevationInterval {
self.start
}
#[must_use]
pub fn end(&self) -> ElevationInterval {
self.end
}
#[must_use]
pub fn sides(&self) -> (ElevationInterval, ElevationInterval) {
(self.left, self.right)
}
#[must_use]
pub fn lowest(&self) -> MeasuredInterval {
self.lowest
}
#[must_use]
pub fn highest(&self) -> MeasuredInterval {
self.highest
}
#[must_use]
pub fn bottom_rise(&self) -> Option<MeasuredInterval> {
self.bottom_rise
}
#[must_use]
pub fn top_rise(&self) -> Option<MeasuredInterval> {
self.top_rise
}
fn plan(&self, direction: MetricDirection, outer: bool) -> Option<ConvexPlanRegion> {
let ((near, far), (low, high)) = if outer {
(
(self.start.lower_metres(), self.end.upper_metres()),
(self.left.lower_metres(), self.right.upper_metres()),
)
} else {
(
(self.start.upper_metres(), self.end.lower_metres()),
(self.left.upper_metres(), self.right.lower_metres()),
)
};
if near >= far || low >= high {
return None;
}
let [dx, dy, _] = direction.components();
let [ax, ay, _] = across(direction).components();
let at = |along: f64, beside: f64| {
[
along.mul_add(dx, beside * ax),
along.mul_add(dy, beside * ay),
]
};
ConvexPlanRegion::try_new(vec![
at(near, low),
at(far, low),
at(far, high),
at(near, high),
])
.ok()
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct HandrailEvidence {
request: HandrailRequest,
parts: Vec<StretchPart>,
pitch: (ElevationInterval, ElevationInterval),
rails: Vec<(ObjectId, RailMeasurement)>,
evidence: Evidence,
}
impl HandrailEvidence {
pub fn try_new(
request: HandrailRequest,
direction: MetricDirection,
pitch: (ElevationInterval, ElevationInterval),
walking_sides: (ElevationInterval, ElevationInterval),
rails: Vec<(ObjectId, RailMeasurement)>,
evidence: Evidence,
) -> Result<Self, WalkingSurfaceError> {
let part = StretchPart::try_new(direction, walking_sides)?;
Self::try_in_parts(request, vec![part], pitch, rails, evidence)
}
pub fn try_in_parts(
request: HandrailRequest,
parts: Vec<StretchPart>,
pitch: (ElevationInterval, ElevationInterval),
mut rails: Vec<(ObjectId, RailMeasurement)>,
evidence: Evidence,
) -> Result<Self, WalkingSurfaceError> {
if parts.is_empty() {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
if parts.len() == 1 {
sides(pitch.0, pitch.1)?;
}
rails.sort_by(|a, b| a.0.cmp(&b.0));
let unique = rails.windows(2).all(|pair| pair[0].0 != pair[1].0);
let requested = rails.iter().all(|(rail, measurement)| {
request.rails.binary_search(rail).is_ok() && measurement.part < parts.len()
});
if !unique || !requested {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
if evidence.exact || evidence.locator.trim().is_empty() {
return Err(WalkingSurfaceError::InexactEvidence);
}
Ok(Self {
request,
parts,
pitch,
rails,
evidence,
})
}
#[must_use]
pub fn request(&self) -> &HandrailRequest {
&self.request
}
#[must_use]
pub fn parts(&self) -> &[StretchPart] {
&self.parts
}
#[must_use]
pub fn direction(&self) -> MetricDirection {
self.first().direction
}
#[must_use]
pub fn pitch(&self) -> (ElevationInterval, ElevationInterval) {
self.pitch
}
#[must_use]
pub fn sides(&self) -> (ElevationInterval, ElevationInterval) {
self.first().sides
}
fn first(&self) -> &StretchPart {
&self.parts[0]
}
fn part(&self, rail: &RailMeasurement) -> &StretchPart {
&self.parts[rail.part]
}
#[must_use]
pub fn rails(&self) -> &[(ObjectId, RailMeasurement)] {
&self.rails
}
#[must_use]
pub fn bottom_extension(&self, rail: &RailMeasurement) -> Option<MeasuredInterval> {
(rail.part == 0).then(|| between(self.pitch.0, rail.start))
}
#[must_use]
pub fn top_extension(&self, rail: &RailMeasurement) -> Option<MeasuredInterval> {
(rail.part + 1 == self.parts.len()).then(|| between(rail.end, self.pitch.1))
}
pub fn side_rail(
&self,
side: RailSide,
) -> Result<Vec<&(ObjectId, RailMeasurement)>, Vec<ObjectId>> {
let mut pieces: Vec<&(ObjectId, RailMeasurement)> = self
.rails
.iter()
.filter(|(_, rail)| self.side(rail) == Some(side))
.collect();
pieces.sort_by(|a, b| {
a.1.part
.cmp(&b.1.part)
.then_with(|| {
a.1.start
.lower_metres()
.total_cmp(&b.1.start.lower_metres())
})
.then_with(|| a.0.cmp(&b.0))
});
let ordered = pieces.windows(2).all(|pair| {
let (lower, upper) = (&pair[0].1, &pair[1].1);
lower.part < upper.part
|| (lower.start.upper_metres() < upper.start.lower_metres()
&& lower.end.upper_metres() < upper.end.lower_metres())
});
if ordered {
Ok(pieces)
} else {
Err(pieces.into_iter().map(|(rail, _)| rail.clone()).collect())
}
}
#[must_use]
pub fn gap(&self, a: &RailMeasurement, b: &RailMeasurement) -> Option<MeasuredInterval> {
let (along_a, along_b) = (self.part(a).direction, self.part(b).direction);
let (outer_a, inner_a) = (a.plan(along_a, true)?, a.plan(along_a, false));
let (outer_b, inner_b) = (b.plan(along_b, true)?, b.plan(along_b, false));
let (inner_a, inner_b) = (inner_a?, inner_b?);
let scale = [&outer_a, &outer_b]
.iter()
.flat_map(|region| region.ring())
.flatten()
.fold(1.0_f64, |scale, value| scale.max(value.abs()));
let margin = 64.0 * f64::EPSILON * scale;
let lower = (outer_a.separation(&outer_b) - margin).max(0.0);
let upper = inner_a.separation(&inner_b).max(0.0) + margin;
MeasuredInterval::try_new(lower, upper.max(lower)).ok()
}
#[must_use]
pub fn side(&self, rail: &RailMeasurement) -> Option<RailSide> {
let (left, right) = self.part(rail).sides;
let low = f64::midpoint(left.lower_metres(), right.lower_metres()).next_down();
let high = f64::midpoint(left.upper_metres(), right.upper_metres()).next_up();
if rail.right.upper_metres() < low {
Some(RailSide::Right)
} else if rail.left.lower_metres() > high {
Some(RailSide::Left)
} else {
None
}
}
#[must_use]
pub fn evidence(&self) -> &Evidence {
&self.evidence
}
}
pub trait WalkingSurfaceService: Send + Sync + 'static {
fn measure_tread_flight(
&self,
request: &TreadFlightRequest,
) -> Result<TreadFlight, WalkingSurfaceError>;
fn measure_sloped_runs(&self, object: &ObjectId) -> Result<SlopedSurface, WalkingSurfaceError>;
fn measure_headroom(&self, request: &HeadroomRequest) -> Result<Headroom, WalkingSurfaceError>;
fn measure_landing(
&self,
request: &LandingRequest,
) -> Result<LandingEvidence, WalkingSurfaceError> {
Err(WalkingSurfaceError::Unsupported(format!(
"landings of {} are not measured by this service",
request.subject()
)))
}
fn measure_clearance_below(
&self,
request: &ClearanceBelowRequest,
) -> Result<ClearanceBelow, WalkingSurfaceError> {
Err(WalkingSurfaceError::Unsupported(format!(
"the clearance below {} is not measured by this service",
request.subject()
)))
}
fn measure_handrails(
&self,
request: &HandrailRequest,
) -> Result<HandrailEvidence, WalkingSurfaceError> {
Err(WalkingSurfaceError::Unsupported(format!(
"handrails along {} are not measured by this service",
request.subject()
)))
}
fn measure_clear_width(
&self,
request: &ClearWidthRequest,
) -> Result<ClearWidthEvidence, WalkingSurfaceError> {
Err(WalkingSurfaceError::Unsupported(format!(
"the clear width along {} is not measured by this service",
request.subject()
)))
}
}
#[derive(Clone)]
pub struct WalkingSurfaceServiceHandle(Arc<dyn WalkingSurfaceService>);
impl WalkingSurfaceServiceHandle {
#[must_use]
pub fn new(service: Arc<dyn WalkingSurfaceService>) -> Self {
Self(service)
}
pub fn measure_tread_flight(
&self,
request: &TreadFlightRequest,
) -> Result<TreadFlight, WalkingSurfaceError> {
let flight = self.0.measure_tread_flight(request)?;
if flight.request() != request {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(flight)
}
pub fn measure_sloped_runs(
&self,
object: &ObjectId,
) -> Result<SlopedSurface, WalkingSurfaceError> {
let ramp = self.0.measure_sloped_runs(object)?;
if ramp.object() != object {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(ramp)
}
pub fn measure_headroom(
&self,
request: &HeadroomRequest,
) -> Result<Headroom, WalkingSurfaceError> {
let headroom = self.0.measure_headroom(request)?;
if headroom.request() != request {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(headroom)
}
pub fn measure_landing(
&self,
request: &LandingRequest,
) -> Result<LandingEvidence, WalkingSurfaceError> {
let landing = self.0.measure_landing(request)?;
if landing.request() != request {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(landing)
}
pub fn measure_clearance_below(
&self,
request: &ClearanceBelowRequest,
) -> Result<ClearanceBelow, WalkingSurfaceError> {
let below = self.0.measure_clearance_below(request)?;
if below.request() != request {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(below)
}
pub fn measure_handrails(
&self,
request: &HandrailRequest,
) -> Result<HandrailEvidence, WalkingSurfaceError> {
let rails = self.0.measure_handrails(request)?;
if rails.request() != request {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(rails)
}
pub fn measure_clear_width(
&self,
request: &ClearWidthRequest,
) -> Result<ClearWidthEvidence, WalkingSurfaceError> {
let width = self.0.measure_clear_width(request)?;
if width.request() != request {
return Err(WalkingSurfaceError::InvalidMeasurement);
}
Ok(width)
}
}
#[cfg(test)]
mod tests {
use super::*;
use axioval_ir::SourceId;
fn source() -> SourceId {
SourceId::new("cad", "m").unwrap()
}
fn id(local: &str) -> ObjectId {
ObjectId::new(source(), local).unwrap()
}
fn point(value: f64) -> ElevationInterval {
ElevationInterval::exact(value).unwrap()
}
fn request(local: &str) -> TreadFlightRequest {
TreadFlightRequest::new(id(local))
}
fn x() -> MetricDirection {
MetricDirection::try_new([1.0, 0.0, 0.0]).unwrap()
}
fn tread(z: f64, front: f64, back: f64) -> Tread {
Tread::try_new(point(z), point(front), point(back)).unwrap()
}
fn evidence(exact: bool) -> Evidence {
Evidence {
source: source(),
locator: "tread-flight:a".into(),
exact,
}
}
fn contains(interval: MeasuredInterval, value: f64) -> bool {
interval.lower() - 1e-15 <= value && value <= interval.upper() + 1e-15
}
#[test]
fn risers_goings_and_nosings_come_from_the_positions() {
let treads = vec![
tread(0.17, 0.0, 0.30),
tread(0.34, 0.28, 0.58),
tread(0.55, 0.56, 0.86),
];
let flight = TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.72),
treads,
evidence(true),
)
.unwrap();
assert!(flight.ends_in_riser());
let risers = flight.risers();
assert_eq!(risers.len(), 4);
let closures = flight
.clone()
.with_final_riser(RiserClosure::Open)
.riser_closures();
assert_eq!(closures.len(), 4);
assert_eq!(closures[3], RiserClosure::Open);
assert_eq!(flight.riser_closures()[3], RiserClosure::NotMeasured);
for (riser, expected) in risers.iter().zip([0.17, 0.17, 0.21, 0.17]) {
assert!(contains(*riser, expected), "{riser:?} {expected}");
assert!(riser.upper() - riser.lower() <= 2.0 * f64::EPSILON);
}
let goings = flight.goings();
assert_eq!(goings.len(), 2);
assert!(goings.iter().all(|going| contains(*going, 0.28)));
assert!(
flight
.nosings()
.iter()
.all(|nosing| contains(*nosing, 0.02))
);
assert!(contains(flight.rise(), 0.72));
assert!(contains(flight.treads()[0].depth(), 0.30));
}
#[test]
fn a_flight_whose_top_is_its_last_tread_has_no_final_riser() {
let treads = vec![tread(0.2, 0.0, 0.3), tread(0.4, 0.3, 0.6)];
let flight = TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.4),
treads,
evidence(true),
)
.unwrap();
assert!(!flight.ends_in_riser());
assert_eq!(flight.risers().len(), 2);
assert!(contains(flight.rise(), 0.4));
}
#[test]
fn incoherent_flights_are_refused() {
let ordered = || vec![tread(0.2, 0.0, 0.3), tread(0.4, 0.3, 0.6)];
let reversed = vec![tread(0.4, 0.3, 0.6), tread(0.2, 0.0, 0.3)];
assert_eq!(
TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.4),
reversed,
evidence(true)
),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.3),
point(0.4),
ordered(),
evidence(true)
),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.3),
ordered(),
evidence(true)
),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.4),
vec![],
evidence(true)
),
Err(WalkingSurfaceError::InvalidMeasurement)
);
let sloped = MetricDirection::try_new([1.0, 0.0, 1.0]).unwrap();
assert_eq!(
TreadFlight::try_new(
request("a"),
WalkingLine::Straight(sloped),
point(0.0),
point(0.4),
ordered(),
evidence(true)
),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.4),
ordered(),
evidence(false)
),
Err(WalkingSurfaceError::InexactEvidence)
);
let widened = ElevationInterval::try_new(0.39, 0.41).unwrap();
assert_eq!(
TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
widened,
ordered(),
evidence(true)
),
Err(WalkingSurfaceError::InvalidMeasurement)
);
}
#[test]
fn a_run_slope_is_its_rise_over_its_length() {
let run = SlopedRun::try_new(x(), point(0.0), point(0.5), point(1.0), point(7.0)).unwrap();
assert!(contains(run.slope(), 0.5 / 6.0));
assert!(run.slope().upper() - run.slope().lower() <= 4.0 * f64::EPSILON);
assert!(contains(run.length(), 6.0) && contains(run.rise(), 0.5));
assert_eq!(
SlopedRun::try_new(x(), point(0.5), point(0.5), point(1.0), point(7.0)),
Err(WalkingSurfaceError::InvalidMeasurement)
);
let ramp = SlopedSurface::try_new(id("a"), vec![run], evidence(true)).unwrap();
assert!(ramp.is_exact());
assert_eq!(
SlopedSurface::try_new(id("a"), vec![], evidence(true)),
Err(WalkingSurfaceError::InvalidMeasurement)
);
}
#[test]
fn headroom_names_requested_obstacles_only() {
let request = HeadroomRequest::new(id("a"), [id("c"), id("a"), id("b"), id("c")]);
assert_eq!(request.obstacles(), &[id("b"), id("c")]);
let clearance = MeasuredInterval::try_new(2.0, 2.0 + 1e-9).ok();
assert!(
Headroom::try_new(request.clone(), clearance, vec![id("b")], evidence(false)).is_ok()
);
assert_eq!(
Headroom::try_new(request.clone(), clearance, vec![id("d")], evidence(false)),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
Headroom::try_new(request.clone(), clearance, vec![], evidence(false)),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
Headroom::try_new(request.clone(), clearance, vec![id("b")], evidence(true)),
Err(WalkingSurfaceError::InexactEvidence)
);
assert!(Headroom::try_new(request, None, vec![], evidence(false)).is_ok());
}
struct Other;
impl WalkingSurfaceService for Other {
fn measure_tread_flight(
&self,
_: &TreadFlightRequest,
) -> Result<TreadFlight, WalkingSurfaceError> {
TreadFlight::try_new(
request("b"),
WalkingLine::Straight(x()),
point(0.0),
point(0.2),
vec![tread(0.2, 0.0, 0.3)],
evidence(true),
)
}
fn measure_sloped_runs(&self, _: &ObjectId) -> Result<SlopedSurface, WalkingSurfaceError> {
let run = SlopedRun::try_new(x(), point(0.0), point(0.5), point(0.0), point(6.0))?;
SlopedSurface::try_new(id("b"), vec![run], evidence(true))
}
fn measure_headroom(&self, _: &HeadroomRequest) -> Result<Headroom, WalkingSurfaceError> {
Headroom::try_new(
HeadroomRequest::new(id("b"), []),
None,
vec![],
evidence(false),
)
}
}
#[test]
fn answers_about_another_object_are_refused() {
let handle = WalkingSurfaceServiceHandle::new(Arc::new(Other));
assert_eq!(
handle.measure_tread_flight(&request("a")),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
handle.measure_sloped_runs(&id("a")),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
handle.measure_headroom(&HeadroomRequest::new(id("a"), [])),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert!(handle.measure_tread_flight(&request("b")).is_ok());
assert!(matches!(
handle.measure_landing(&LandingRequest::new(id("b"), WalkingEnd::FlightTop, [])),
Err(WalkingSurfaceError::Unsupported(_))
));
assert!(matches!(
handle.measure_clearance_below(&ClearanceBelowRequest::new(id("b"), [])),
Err(WalkingSurfaceError::Unsupported(_))
));
let rails =
HandrailRequest::try_new(id("b"), WalkingStretch::Flight, [], (0.1, 1.5), 0.3).unwrap();
assert!(matches!(
handle.measure_handrails(&rails),
Err(WalkingSurfaceError::Unsupported(_))
));
}
fn rail(left: f64, right: f64, lowest: f64) -> RailMeasurement {
RailMeasurement::try_new(
(point(-0.3), point(1.5)),
(point(left), point(right)),
MeasuredInterval::try_new(lowest, lowest + 1e-9).unwrap(),
MeasuredInterval::try_new(lowest + 0.01, lowest + 0.01 + 1e-9).unwrap(),
)
.unwrap()
}
#[test]
fn handrails_are_requested_rails_on_a_side_with_extensions() {
let request = HandrailRequest::try_new(
id("a"),
WalkingStretch::Flight,
[id("r"), id("a"), id("l"), id("r")],
(0.2, 1.5),
0.3,
)
.unwrap();
assert_eq!(request.rails(), &[id("l"), id("r")]);
assert!(
HandrailRequest::try_new(id("a"), WalkingStretch::Flight, [], (-0.1, 1.5), 0.0)
.is_err()
);
let evidence = |exact: bool| Evidence {
source: source(),
locator: "handrails:a".into(),
exact,
};
let measure = |rails: Vec<(ObjectId, RailMeasurement)>, exact: bool| {
HandrailEvidence::try_new(
request.clone(),
x(),
(point(0.0), point(1.12)),
(point(0.0), point(1.2)),
rails,
evidence(exact),
)
};
let rails = vec![
(id("r"), rail(-0.05, 0.0, 0.9)),
(id("l"), rail(1.2, 1.25, 0.85)),
];
let measured = measure(rails.clone(), false).unwrap();
assert_eq!(measured.rails()[0].0, id("l"));
let (left, right) = (measured.rails()[0].1, measured.rails()[1].1);
assert_eq!(measured.side(&left), Some(RailSide::Left));
assert_eq!(measured.side(&right), Some(RailSide::Right));
assert_eq!(measured.side(&rail(0.5, 0.7, 0.9)), None);
assert!(contains(measured.bottom_extension(&left).unwrap(), 0.3));
assert!(contains(measured.top_extension(&left).unwrap(), 0.38));
assert_eq!(
measure(rails, true),
Err(WalkingSurfaceError::InexactEvidence)
);
for rails in [
vec![(id("x"), rail(0.0, 0.1, 0.9))],
vec![
(id("l"), rail(0.0, 0.1, 0.9)),
(id("l"), rail(0.0, 0.1, 0.9)),
],
] {
assert_eq!(
measure(rails, false),
Err(WalkingSurfaceError::InvalidMeasurement)
);
}
let rise = MeasuredInterval::try_new(-0.1, 0.0).ok();
assert!(rail(0.0, 0.1, 0.9).with_rises(rise, None).is_err());
assert!(
RailMeasurement::try_new(
(point(0.0), point(1.0)),
(point(0.0), point(0.1)),
MeasuredInterval::try_new(1.0, 1.0).unwrap(),
MeasuredInterval::try_new(0.9, 0.9).unwrap(),
)
.is_err()
);
}
fn piece(start: f64, end: f64, left: f64, right: f64) -> RailMeasurement {
RailMeasurement::try_new(
(point(start), point(end)),
(point(left), point(right)),
MeasuredInterval::try_new(0.9, 0.9 + 1e-9).unwrap(),
MeasuredInterval::try_new(0.9, 0.9 + 1e-9).unwrap(),
)
.unwrap()
}
#[test]
fn the_pieces_along_a_side_are_ordered_with_their_gaps() {
let request = HandrailRequest::try_new(
id("a"),
WalkingStretch::Flight,
[id("p"), id("q"), id("r"), id("s"), id("m")],
(0.2, 1.5),
0.3,
)
.unwrap();
let measure = |rails: Vec<(ObjectId, RailMeasurement)>| {
HandrailEvidence::try_new(
request.clone(),
x(),
(point(0.0), point(0.84)),
(point(0.0), point(1.2)),
rails,
evidence(false),
)
.unwrap()
};
let measured = measure(vec![
(id("p"), piece(0.5, 1.14, 1.25, 1.3)),
(id("q"), piece(-0.3, 0.4, 1.25, 1.3)),
(id("r"), piece(-0.3, 0.4, -0.1, -0.05)),
(id("s"), piece(0.4, 1.14, -0.1, -0.05)),
(id("m"), piece(-0.3, 1.14, 0.55, 0.65)),
]);
let left = measured.side_rail(RailSide::Left).unwrap();
let names: Vec<&ObjectId> = left.iter().map(|(rail, _)| rail).collect();
assert_eq!(names, [&id("q"), &id("p")]);
let gap = measured.gap(&left[0].1, &left[1].1).unwrap();
assert!(
contains(gap, 0.1) && gap.upper() - gap.lower() < 1e-12,
"{gap:?}"
);
let right = measured.side_rail(RailSide::Right).unwrap();
let gap = measured.gap(&right[0].1, &right[1].1).unwrap();
assert!(gap.lower() == 0.0 && gap.upper() < 1e-12, "{gap:?}");
let apart = measured
.gap(&piece(-0.3, 0.4, 1.25, 1.3), &piece(0.7, 1.14, 1.6, 1.65))
.unwrap();
assert!(contains(apart, 0.3_f64.hypot(0.3)), "{apart:?}");
let nested = measure(vec![
(id("p"), piece(-0.3, 1.14, 1.25, 1.3)),
(id("q"), piece(0.0, 0.84, 1.3, 1.35)),
]);
assert_eq!(
nested.side_rail(RailSide::Left),
Err(vec![id("p"), id("q")])
);
assert_eq!(nested.side_rail(RailSide::Right), Ok(vec![]));
let blurred = RailMeasurement::try_new(
(around(0.5, 0.2), around(0.6, 0.2)),
(point(1.25), point(1.3)),
MeasuredInterval::try_new(0.9, 0.9).unwrap(),
MeasuredInterval::try_new(0.9, 0.9).unwrap(),
)
.unwrap();
assert_eq!(measured.gap(&left[0].1, &blurred), None);
}
#[test]
fn a_turning_flights_rails_are_measured_part_by_part() {
let y = MetricDirection::try_new([0.0, 1.0, 0.0]).unwrap();
let parts = vec![
StretchPart::try_new(x(), (point(0.0), point(1.0))).unwrap(),
StretchPart::try_new(y, (point(-1.84), point(-0.84))).unwrap(),
];
let request = HandrailRequest::try_new(
id("a"),
WalkingStretch::Flight,
[id("p"), id("q"), id("r")],
(0.2, 1.5),
0.3,
)
.unwrap();
let measure = |rails: Vec<(ObjectId, RailMeasurement)>| {
HandrailEvidence::try_in_parts(
request.clone(),
parts.clone(),
(point(0.0), point(1.56)),
rails,
evidence(false),
)
};
let measured = measure(vec![
(id("p"), piece(-0.3, 1.89, -0.1, -0.05)),
(id("q"), piece(-0.1, 1.86, -1.89, -1.84).in_part(1)),
(id("r"), piece(1.0, 1.86, -0.84, -0.79).in_part(1)),
])
.unwrap();
assert_eq!(measured.parts().len(), 2);
let (p, q, r) = (
&measured.rails()[0].1,
&measured.rails()[1].1,
&measured.rails()[2].1,
);
assert_eq!(measured.side(p), Some(RailSide::Right));
assert_eq!(measured.side(q), Some(RailSide::Right));
assert_eq!(measured.side(r), Some(RailSide::Left));
let outer = measured.side_rail(RailSide::Right).unwrap();
let names: Vec<&ObjectId> = outer.iter().map(|(rail, _)| rail).collect();
assert_eq!(names, [&id("p"), &id("q")]);
let gap = measured.gap(p, q).unwrap();
assert!(gap.lower() == 0.0 && gap.upper() < 1e-12, "{gap:?}");
assert!(contains(measured.bottom_extension(p).unwrap(), 0.3));
assert_eq!(measured.top_extension(p), None);
assert!(contains(measured.top_extension(q).unwrap(), 0.3));
assert_eq!(measured.bottom_extension(q), None);
assert_eq!(
measure(vec![(id("p"), piece(0.0, 1.0, -0.1, -0.05).in_part(2))]),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
HandrailEvidence::try_in_parts(
request.clone(),
vec![],
(point(0.0), point(1.56)),
vec![],
evidence(false)
),
Err(WalkingSurfaceError::InvalidMeasurement)
);
let sloped = MetricDirection::try_new([1.0, 0.0, 1.0]).unwrap();
assert!(StretchPart::try_new(sloped, (point(0.0), point(1.0))).is_err());
}
fn around(value: f64, margin: f64) -> ElevationInterval {
ElevationInterval::try_new(value - margin, value + margin).unwrap()
}
#[test]
fn widths_come_from_the_sides_and_the_narrowest_tread_governs() {
let sided = |z: f64, front: f64, right: f64| {
tread(z, front, front + 0.3)
.with_sides(point(0.0), point(right))
.unwrap()
};
let treads = vec![sided(0.2, 0.0, 1.2), sided(0.4, 0.3, 1.1)];
let flight = TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.4),
treads,
evidence(true),
)
.unwrap();
assert!(contains(flight.width().unwrap(), 1.1));
let treads = vec![sided(0.2, 0.0, 1.2), tread(0.4, 0.3, 0.6)];
let flight = TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.4),
treads,
evidence(true),
)
.unwrap();
assert_eq!(flight.width(), None);
assert_eq!(
tread(0.2, 0.0, 0.3).with_sides(point(1.0), point(0.0)),
Err(WalkingSurfaceError::InvalidMeasurement)
);
let widened = tread(0.2, 0.0, 0.3)
.with_sides(point(0.0), ElevationInterval::try_new(1.0, 1.1).unwrap())
.unwrap();
assert!(!widened.is_exact());
assert_eq!(
TreadFlight::try_new(
request("a"),
WalkingLine::Straight(x()),
point(0.0),
point(0.2),
vec![widened],
evidence(true)
),
Err(WalkingSurfaceError::InexactEvidence)
);
let run = SlopedRun::try_new(x(), point(0.0), point(0.5), point(1.0), point(7.0))
.unwrap()
.with_sides(point(-0.75), point(0.75))
.unwrap();
assert!(contains(run.width().unwrap(), 1.5) && run.is_exact());
let [ax, ay, _] = across(x()).components();
assert!(ax.abs() < f64::EPSILON && (ay - 1.0).abs() < f64::EPSILON);
}
fn landing_evidence(
request: &LandingRequest,
landing: Option<Landing>,
exact: bool,
) -> Result<LandingEvidence, WalkingSurfaceError> {
LandingEvidence::try_new(
request.clone(),
x(),
point(1.0),
landing,
Evidence {
source: source(),
locator: "landing:a".into(),
exact,
},
)
}
#[test]
fn a_landing_is_carried_by_a_requested_object_beyond_the_edge() {
let request = LandingRequest::new(id("a"), WalkingEnd::FlightTop, [id("c"), id("a")]);
assert_eq!(request.candidates(), &[id("c")]);
let extent = LandingExtent::try_new(point(2.5), point(-0.1), point(1.4)).unwrap();
let found =
landing_evidence(&request, Some(Landing::new(id("c"), Some(extent))), true).unwrap();
assert!(contains(found.depth().unwrap(), 1.5));
assert!(contains(found.width().unwrap(), 1.5));
assert!(
landing_evidence(&request, Some(Landing::new(id("a"), Some(extent))), true).is_ok()
);
assert_eq!(
landing_evidence(&request, Some(Landing::new(id("d"), None)), true),
Err(WalkingSurfaceError::InvalidMeasurement)
);
let short = LandingExtent::try_new(point(0.5), point(0.0), point(1.0)).unwrap();
assert_eq!(
landing_evidence(&request, Some(Landing::new(id("c"), Some(short))), true),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
landing_evidence(&request, None, false),
Err(WalkingSurfaceError::InexactEvidence)
);
assert_eq!(
LandingExtent::try_new(point(2.0), point(1.0), point(0.0)),
Err(WalkingSurfaceError::InvalidMeasurement)
);
let unmeasured =
landing_evidence(&request, Some(Landing::new(id("c"), None)), true).unwrap();
assert_eq!(unmeasured.depth(), None);
assert_eq!(unmeasured.width(), None);
}
#[test]
fn the_clearance_below_names_requested_spaces_only() {
let request = ClearanceBelowRequest::new(id("a"), [id("s"), id("a"), id("s")]);
assert_eq!(request.spaces(), &[id("s")]);
let clearance = MeasuredInterval::try_new(1.5, 1.5 + 1e-9).ok();
assert!(
ClearanceBelow::try_new(request.clone(), clearance, vec![id("s")], evidence(false))
.is_ok()
);
assert_eq!(
ClearanceBelow::try_new(request.clone(), clearance, vec![id("t")], evidence(false)),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
ClearanceBelow::try_new(request.clone(), clearance, vec![id("s")], evidence(true)),
Err(WalkingSurfaceError::InexactEvidence)
);
assert!(ClearanceBelow::try_new(request, None, vec![], evidence(false)).is_ok());
}
#[test]
fn winder_angles_and_widths_come_from_nosings_and_sides() {
let nosing = |degrees: f64| {
let (sin, cos) = degrees.to_radians().sin_cos();
PlanSegment::try_new([0.0, 0.0], [cos, sin], 0.0).unwrap()
};
let treads: Vec<Tread> = [0.0, 30.0, 60.0, 90.0]
.iter()
.enumerate()
.map(|(step, degrees)| {
#[allow(clippy::cast_precision_loss)]
let step = step as f64;
tread(0.18 * (step + 1.0), 0.3 * step, 0.3 * step + 0.3)
.with_nosing(nosing(*degrees))
.with_sides(point(-0.4), point(0.5))
.unwrap()
.with_riser_below(RiserClosure::Closed)
})
.collect();
let line = WalkingLine::Turning(vec![[0.5, 0.1], [0.4, 0.3], [0.3, 0.4], [0.1, 0.5]]);
let flight = TreadFlight::try_new(
request("a"),
line.clone(),
point(0.0),
point(0.72),
treads,
evidence(false),
)
.unwrap_err();
assert_eq!(flight, WalkingSurfaceError::InexactEvidence);
let treads: Vec<Tread> = [0.0, 30.0, 60.0, 90.0]
.iter()
.enumerate()
.map(|(step, degrees)| {
#[allow(clippy::cast_precision_loss)]
let step = step as f64;
let tread = tread(0.18 * (step + 1.0), 0.3 * step, 0.3 * step + 0.3)
.with_nosing(nosing(*degrees));
if step == 0.0 {
tread.with_sides(point(-0.4), point(0.5)).unwrap()
} else {
tread
}
})
.collect();
let flight = TreadFlight::try_new(
request("a"),
line,
point(0.0),
point(0.72),
treads,
evidence(true),
)
.unwrap();
assert!(flight.walking_line().is_turning());
let angles = flight.winder_angles();
assert_eq!(angles.len(), 3);
for angle in angles {
let angle = angle.unwrap();
assert!(contains(angle, 30.0_f64.to_radians()), "{angle:?}");
assert!(angle.upper() - angle.lower() < 1e-12);
}
let width = flight.treads()[0].width().unwrap();
assert!(contains(width, 0.9));
assert_eq!(flight.treads()[1].width(), None);
assert_eq!(flight.width(), None);
assert_eq!(flight.treads()[0].riser_below(), RiserClosure::NotMeasured);
}
#[test]
fn an_uncertain_nosing_widens_its_angle_and_a_short_one_has_none() {
let a = PlanSegment::try_new([0.0, 0.0], [1.0, 0.0], 0.001).unwrap();
let b = PlanSegment::try_new([0.0, 0.0], [0.0, 1.0], 0.001).unwrap();
let square = a.angle_to(&b).unwrap();
assert!(square.upper() <= std::f64::consts::FRAC_PI_2);
assert!(square.lower() < std::f64::consts::FRAC_PI_2 - 0.0039);
let parallel = a.angle_to(&a).unwrap();
assert!(parallel.lower() == 0.0 && parallel.upper() > 0.0039);
let short = PlanSegment::try_new([0.0, 0.0], [0.001, 0.0], 0.001).unwrap();
assert_eq!(short.angle_to(&a), None);
assert_eq!(
PlanSegment::try_new([0.0, 0.0], [0.0, 0.0], 0.0),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
PlanSegment::try_new([0.0, 0.0], [1.0, 0.0], -1.0),
Err(WalkingSurfaceError::InvalidMeasurement)
);
}
#[test]
fn clear_widths_name_requested_obstacles_and_a_band_above_the_pitch_line() {
let request = ClearWidthRequest::try_new(
id("f"),
WalkingStretch::Flight,
[id("rail"), id("f"), id("rail")],
(0.5, 1.5),
)
.unwrap();
assert_eq!(request.obstacles(), [id("rail")]);
assert_eq!(request.band(), (0.5, 1.5));
for band in [(1.5, 0.5), (-0.1, 1.0), (0.5, f64::NAN)] {
assert!(ClearWidthRequest::try_new(id("f"), WalkingStretch::Flight, [], band).is_err());
}
let width = MeasuredInterval::try_new(0.99, 1.01).unwrap();
let measured =
ClearWidthEvidence::try_new(request.clone(), width, vec![id("rail")], evidence(false))
.unwrap();
assert_eq!(measured.governing(), [id("rail")]);
assert_eq!(
ClearWidthEvidence::try_new(request.clone(), width, vec![id("wall")], evidence(false)),
Err(WalkingSurfaceError::InvalidMeasurement)
);
assert_eq!(
ClearWidthEvidence::try_new(request, width, vec![], evidence(true)),
Err(WalkingSurfaceError::InexactEvidence)
);
}
#[test]
fn walking_lines_and_offsets_are_checked() {
assert!(TreadFlightRequest::from_inner_side(id("a"), 0.0).is_err());
assert!(TreadFlightRequest::from_inner_side(id("a"), f64::NAN).is_err());
let request = TreadFlightRequest::from_inner_side(id("a"), 0.4).unwrap();
assert_eq!(
request.walking_line(),
WalkingLinePlacement::FromInnerSide(0.4)
);
let treads = || vec![tread(0.2, 0.0, 0.3), tread(0.4, 0.3, 0.6)];
for line in [
WalkingLine::Turning(vec![[0.0, 0.0]]),
WalkingLine::Turning(vec![[0.0, 0.0], [0.0, 0.0]]),
WalkingLine::Turning(vec![[0.0, 0.0], [f64::INFINITY, 0.0]]),
] {
assert_eq!(
TreadFlight::try_new(
request.clone(),
line,
point(0.0),
point(0.4),
treads(),
evidence(true)
),
Err(WalkingSurfaceError::InvalidMeasurement)
);
}
assert!(
tread(0.2, 0.0, 0.3)
.with_sides(point(1.0), point(0.0))
.is_err()
);
}
}