rustsim_mobility/

obstacle.rs

//! Cross-domain obstacle trait.
//!
//! The critical challenge in a coupled crowd-plus-traffic simulation is
//! that crowd models must see vehicles as moving obstacles, and vehicle
//! models must see pedestrians as obstacles. Neither domain crate should
//! depend on the other.
//!
//! This module defines a tiny abstract interface both sides can
//! implement in thin adapter types.

use rustsim_geometry::vec3::Vec3;
use rustsim_vehicle::{Vehicle, VehicleClass};

/// Kind of an obstacle, used for priority and styling.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum ObstacleKind {
    /// Individual pedestrian or cyclist body.
    Pedestrian,
    /// Road vehicle (car, taxi, van).
    Vehicle,
    /// Public-transit vehicle (bus, tram, metro, train).
    Transit,
    /// Static fixed object (kerb, bollard, furniture).
    Static,
}

/// Instantaneous snapshot of an obstacle's bounding circle and velocity
/// in 3-D. Implementations should project to `z = 0` if they live in 2-D.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ObstacleSnapshot {
    /// Stable identifier within the hosting simulation.
    pub id: u64,
    /// Kind.
    pub kind: ObstacleKind,
    /// Centre in 3-D.
    pub center: Vec3,
    /// Enclosing sphere radius in metres.
    pub radius: f64,
    /// Linear velocity in 3-D (m/s).
    pub velocity: Vec3,
}

/// Any entity that can expose itself as an obstacle to foreign domains.
pub trait Obstacle {
    /// Produce an instantaneous snapshot for broadphase insertion.
    fn snapshot(&self) -> ObstacleSnapshot;
}

impl Obstacle for ObstacleSnapshot {
    #[inline]
    fn snapshot(&self) -> ObstacleSnapshot {
        *self
    }
}

/// Thin obstacle adapter for a [`rustsim_crowd::Pedestrian`].
pub struct PedestrianObstacle<'a> {
    /// Stable identifier supplied by the hosting simulation.
    pub id: u64,
    /// Pedestrian state to expose as an obstacle.
    pub pedestrian: &'a rustsim_crowd::Pedestrian,
    /// Vertical coordinate to attach to the 2-D pedestrian state.
    pub z: f64,
}

impl<'a> PedestrianObstacle<'a> {
    /// Construct a pedestrian obstacle at ground level.
    pub fn ground(id: u64, pedestrian: &'a rustsim_crowd::Pedestrian) -> Self {
        Self {
            id,
            pedestrian,
            z: 0.0,
        }
    }

    /// Construct a pedestrian obstacle at a specific vertical coordinate.
    pub fn at_z(id: u64, pedestrian: &'a rustsim_crowd::Pedestrian, z: f64) -> Self {
        Self { id, pedestrian, z }
    }
}

impl Obstacle for PedestrianObstacle<'_> {
    fn snapshot(&self) -> ObstacleSnapshot {
        ObstacleSnapshot {
            id: self.id,
            kind: ObstacleKind::Pedestrian,
            center: [self.pedestrian.pos[0], self.pedestrian.pos[1], self.z],
            radius: self.pedestrian.radius,
            velocity: [self.pedestrian.vel[0], self.pedestrian.vel[1], 0.0],
        }
    }
}

/// Thin obstacle adapter for a [`rustsim_vehicle::Vehicle`].
pub struct VehicleObstacle<'a> {
    /// Stable identifier supplied by the hosting simulation.
    pub id: u64,
    /// Vehicle state to expose as an obstacle.
    pub vehicle: &'a Vehicle,
    /// Vertical coordinate to attach to the road-plane vehicle state.
    pub z: f64,
}

impl<'a> VehicleObstacle<'a> {
    /// Construct a vehicle obstacle at ground level.
    pub fn ground(id: u64, vehicle: &'a Vehicle) -> Self {
        Self {
            id,
            vehicle,
            z: 0.0,
        }
    }

    /// Construct a vehicle obstacle at a specific vertical coordinate.
    pub fn at_z(id: u64, vehicle: &'a Vehicle, z: f64) -> Self {
        Self { id, vehicle, z }
    }
}

impl Obstacle for VehicleObstacle<'_> {
    fn snapshot(&self) -> ObstacleSnapshot {
        let (sin_heading, cos_heading) = self.vehicle.heading.sin_cos();
        let center_x = self.vehicle.pos[0] + cos_heading * self.vehicle.length * 0.5;
        let center_y = self.vehicle.pos[1] + sin_heading * self.vehicle.length * 0.5;
        let radius = 0.5 * self.vehicle.length.hypot(self.vehicle.width);
        let kind = match self.vehicle.class {
            VehicleClass::Bus | VehicleClass::Rail => ObstacleKind::Transit,
            _ => ObstacleKind::Vehicle,
        };
        ObstacleSnapshot {
            id: self.id,
            kind,
            center: [center_x, center_y, self.z],
            radius,
            velocity: [
                cos_heading * self.vehicle.speed,
                sin_heading * self.vehicle.speed,
                0.0,
            ],
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn snapshot_is_self_describing() {
        let s = ObstacleSnapshot {
            id: 1,
            kind: ObstacleKind::Vehicle,
            center: [1.0, 2.0, 0.0],
            radius: 1.2,
            velocity: [3.0, 0.0, 0.0],
        };
        assert_eq!(s.snapshot(), s);
    }

    #[test]
    fn adapters_project_domain_models_to_obstacles() {
        let ped = rustsim_crowd::Pedestrian::new([1.0, 2.0], [0.5, 0.0], 0.25, 1.3, [5.0, 2.0]);
        let snap = PedestrianObstacle::ground(7, &ped).snapshot();
        assert_eq!(snap.kind, ObstacleKind::Pedestrian);
        assert_eq!(snap.center, [1.0, 2.0, 0.0]);
        assert_eq!(snap.radius, 0.25);

        let mut vehicle = Vehicle::default_bus();
        vehicle.pos = [10.0, 0.0];
        vehicle.speed = 3.0;
        let snap = VehicleObstacle::ground(8, &vehicle).snapshot();
        assert_eq!(snap.kind, ObstacleKind::Transit);
        assert!(snap.center[0] > vehicle.pos[0]);
        assert_eq!(snap.velocity, [3.0, 0.0, 0.0]);
    }
}