diff --git a/src/agents/vehicle.rs b/src/agents/vehicle.rs index 58a9732..ce36775 100644 --- a/src/agents/vehicle.rs +++ b/src/agents/vehicle.rs @@ -90,6 +90,8 @@ pub struct Vehicle { pub min_safe_distance: i32, /// Final cell for the vehicle's trip pub destination: CellID, + /// Final trip destination (immutable, used for completion check; destination may change for transit cells) + pub trip_destination: CellID, /// A boolean indicating if the vehicle is a confclict participant pub is_conflict_participant: bool, @@ -162,6 +164,7 @@ impl Vehicle { bearing: 0.0, min_safe_distance: 0, destination: -1, + trip_destination: -1, is_conflict_participant: false, trip: -1, transits_made: 0, @@ -672,6 +675,7 @@ impl VehicleBuilder { /// ``` pub fn with_destination(mut self, cell_id: CellID) -> Self { self.vehicle.destination = cell_id; + self.vehicle.trip_destination = cell_id; self } diff --git a/src/intentions/intention.rs b/src/intentions/intention.rs index 156ea13..f5be0e0 100644 --- a/src/intentions/intention.rs +++ b/src/intentions/intention.rs @@ -3,7 +3,7 @@ use crate::behaviour::BehaviourType; use crate::agents::{ TailIntentionManeuver, Vehicle, VehicleError, VehicleID, VehicleIntention, }; -use crate::grid::cell::CellState; +use crate::grid::cell::{Cell, CellState}; use crate::maneuver::LaneChangeType; use crate::grid::{cell::CellID, road_network::GridRoads}; use crate::intentions::{intention_type::IntentionType, Intentions}; @@ -238,6 +238,27 @@ pub fn find_intention<'a>( return Ok(result); } + // Шf stopped at red traffic light do early return + let forward_cell_id = source_cell.get_forward_id(); + if forward_cell_id > 0 { + if let Some(forward_cell) = net.get_cell(&forward_cell_id) { + if forward_cell.get_state() == CellState::Banned { + let result = VehicleIntention { + intention_maneuver: LaneChangeType::Block, + intention_speed: 0, + destination: None, + confusion: None, + intention_cell_id: vehicle.cell_id, + tail_intention_cells: vec![], + intermediate_cells: Vec::with_capacity(0), + tail_maneuver: tail_maneuver, + should_stop: false, + }; + return Ok(result); + } + } + } + // Vehicle's speed should not be greater than speed limit let mut intention_speed = vehicle.speed.min(speed_limit); @@ -263,14 +284,14 @@ pub fn find_intention<'a>( } // Considering that vehicle always wants to accelerate: - let observe_distance = speed_possible + vehicle.min_safe_distance; + let _observe_distance = speed_possible + vehicle.min_safe_distance; // Check if maneuvers are allowed (they could be prohibeted due the vehicle's tail is not done previous maneuver yet) let maneuvers_allowed = vehicle.timer_non_maneuvers <= 0 && tail_maneuver.intention_maneuver != LaneChangeType::ChangeRight && tail_maneuver.intention_maneuver != LaneChangeType::ChangeLeft; - let mut destination: Option = None; + let destination: Option = None; let mut confusion: Option = None; // println!( @@ -293,7 +314,8 @@ pub fn find_intention<'a>( source_cell, net, maneuvers_allowed, - observe_distance + 1, + 1000, + // observe_distance + 1, // depth-limited ) { Ok(path) => path, Err(e) => { @@ -313,9 +335,16 @@ pub fn find_intention<'a>( target_cell, net, maneuvers_allowed, - Some(observe_distance + 1), + None, + // Some(observe_distance + 1), // depth-limited ) { - Ok(path) => path, + Ok(path) => { + // Clear confusion if vehicle was previously in confusion mode and found path + if vehicle.confusion { + confusion = Some(false); + } + path + }, Err(e) if e != shortest_path::router::AStarError::NoPathFound { start_id: source_cell.get_id(), @@ -329,7 +358,7 @@ pub fn find_intention<'a>( Ok(path) => path, Err(e) => return Err(IntentionError::NoPathForNoRoute(e)), }; - destination = Some(new_path.vertices()[new_path.vertices().len() - 1].get_id()); + // Do NOT overwrite destination - keep original trip destination intention_speed = 1; speed_possible = intention_speed; confusion = Some(true); @@ -431,6 +460,70 @@ const UNDEFINED_MANEUVER: LaneChangeType = LaneChangeType::ChangeRight; /// Attempts to find an alternate maneuver (lane change) for a blocked vehicle. /// +/// Helper: Creates a block intention +fn create_block_intention(cell_id: CellID, should_stop: bool) -> VehicleIntention { + VehicleIntention { + intention_maneuver: LaneChangeType::Block, + intention_speed: 0, + destination: None, + confusion: None, + intention_cell_id: cell_id, + tail_intention_cells: vec![], + intermediate_cells: Vec::with_capacity(0), + tail_maneuver: TailIntentionManeuver::default(), + should_stop, + } +} + +/// Helper: Checks if alternate path (left or right) is available and calculates cost +fn check_alternate_direction( + cell_id: CellID, + source_cell: &Cell, + target_cell: &Cell, + net: &GridRoads, + current_state: &HashMap, + direction: &str, + max_depth: Option, +) -> Result<(CellID, f64), IntentionError> { + if cell_id <= 0 { + return Ok((-1, INFINITY)); + } + + let cell = net.get_cell(&cell_id).ok_or_else(|| { + if direction == "left" { + IntentionError::NoLeftCell(source_cell.get_id()) + } else { + IntentionError::NoRightCell(source_cell.get_id()) + } + })?; + + let is_blocked = current_state + .get(&cell_id) + .map(|&id| id > 0) + .unwrap_or(false); + + if is_blocked || cell.get_state() != CellState::Free { + return Ok((-1, INFINITY)); + } + + match shortest_path(cell, target_cell, net, true, max_depth) { + Ok(path) => { + let cost = path.cost() + source_cell.distance_to(cell); + Ok((cell_id, cost)) + } + Err(shortest_path::router::AStarError::NoPathFound { .. }) => { + Ok((-1, INFINITY)) + } + Err(_) => { + if direction == "left" { + Err(IntentionError::LeftPathFind(cell_id)) + } else { + Err(IntentionError::RightPathFind(cell_id)) + } + } + } +} + /// If the vehicle cannot move forward, tries left or right lane changes /// and selects the best available option. /// @@ -447,18 +540,7 @@ pub fn find_alternate_intention<'a>( // If maneuvers are not allowed (tail still completing previous maneuver), block immediately if !maneuvers_allowed { - let result = VehicleIntention { - intention_maneuver: LaneChangeType::Block, - intention_speed: 0, - destination: None, - confusion: None, - intention_cell_id: source_cell_id, - tail_intention_cells: vec![], - intermediate_cells: Vec::with_capacity(0), - tail_maneuver: TailIntentionManeuver::default(), - should_stop: false, - }; - return Ok(result); + return Ok(create_block_intention(source_cell_id, false)); } let source_cell = net @@ -469,103 +551,52 @@ pub fn find_alternate_intention<'a>( .get_cell(&target_cell_id) .ok_or(IntentionError::NoTargetCell(target_cell_id))?; - let mut min_left_dist = INFINITY; - let mut min_right_dist = INFINITY; - - // Check left maneuver - let mut left_cell_id = source_cell.get_left_id(); - if left_cell_id > 0 { - let left_cell = net - .get_cell(&left_cell_id) - .ok_or(IntentionError::NoLeftCell(vehicle.cell_id))?; - - // Check if possible maneuver can't be made - let is_blocked = current_state - .get(&left_cell_id) - .map(|&id| id > 0) - .unwrap_or(false); - - if !is_blocked && left_cell.get_state() == CellState::Free { - match shortest_path(left_cell, target_cell, net, true, Some(vehicle.speed)) { - Ok(path) => { - let cost = path.cost(); - min_left_dist = cost + source_cell.distance_to(left_cell); - } - Err(e) - if e != shortest_path::router::AStarError::NoPathFound { - start_id: left_cell_id, - end_id: target_cell_id, - } => - { - return Err(IntentionError::LeftPathFind(left_cell_id)) - } - Err(_) => { - min_left_dist = INFINITY; - } - } - } else { - left_cell_id = -1; - } - } - - // Check right maneuver - let mut right_cell_id = source_cell.get_right_id(); - if right_cell_id > 0 { - let right_cell = net - .get_cell(&right_cell_id) - .ok_or(IntentionError::NoRightCell(vehicle.cell_id))?; + // Check left and right alternate paths (no depth limit to see full route) + let (left_cell_id, min_left_dist) = check_alternate_direction( + source_cell.get_left_id(), + source_cell, + target_cell, + net, + current_state, + "left", + None, + // Some(vehicle.speed), // depth-limited + )?; - let is_blocked = current_state - .get(&right_cell_id) - .map(|&id| id > 0) - .unwrap_or(false); + let (right_cell_id, min_right_dist) = check_alternate_direction( + source_cell.get_right_id(), + source_cell, + target_cell, + net, + current_state, + "right", + None, + // Some(vehicle.speed), // depth-limited + )?; - if !is_blocked && right_cell.get_state() == CellState::Free { - match shortest_path(right_cell, target_cell, net, true, Some(vehicle.speed)) { - Ok(path) => { - let cost = path.cost(); - min_right_dist = cost + source_cell.distance_to(right_cell); - } - Err(e) - if e != shortest_path::router::AStarError::NoPathFound { - start_id: right_cell_id, - end_id: target_cell_id, - } => - { - return Err(IntentionError::RightPathFind(right_cell_id)) - } - Err(_) => { - min_right_dist = INFINITY; - } - } - } else { - right_cell_id = -1; - } + // If both paths are impossible (infinite distance), don't attempt a lane change - just block + if min_left_dist == INFINITY && min_right_dist == INFINITY { + return Ok(create_block_intention(source_cell_id, true)); } - // Choose best maneuver - let mut min_cell: CellID; - let mut intention_maneuver: LaneChangeType; - if UNDEFINED_MANEUVER == LaneChangeType::ChangeRight { - min_cell = right_cell_id; - intention_maneuver = LaneChangeType::ChangeRight; - if min_left_dist < min_right_dist { - min_cell = left_cell_id; - intention_maneuver = LaneChangeType::ChangeLeft; - } + // Choose best maneuver based on distance comparison + let (min_cell, intention_maneuver) = if min_left_dist < min_right_dist { + (left_cell_id, LaneChangeType::ChangeLeft) + } else if min_right_dist < min_left_dist { + (right_cell_id, LaneChangeType::ChangeRight) } else { - min_cell = left_cell_id; - intention_maneuver = LaneChangeType::ChangeLeft; - if min_right_dist < min_left_dist { - min_cell = right_cell_id; - intention_maneuver = LaneChangeType::ChangeRight; + // Equal distances - use UNDEFINED_MANEUVER as tiebreaker + if UNDEFINED_MANEUVER == LaneChangeType::ChangeRight { + (right_cell_id, LaneChangeType::ChangeRight) + } else { + (left_cell_id, LaneChangeType::ChangeLeft) } - } + }; - // Apply the chosen maneuver + // Apply the chosen maneuver if valid if min_cell > 0 { - let result = VehicleIntention { - intention_maneuver: intention_maneuver, + return Ok(VehicleIntention { + intention_maneuver, intention_speed: 1, destination: None, confusion: None, @@ -574,22 +605,9 @@ pub fn find_alternate_intention<'a>( intermediate_cells: Vec::with_capacity(0), tail_maneuver: TailIntentionManeuver::default(), should_stop: true, - }; - return Ok(result); + }) } - let result = VehicleIntention { - intention_maneuver: LaneChangeType::Block, - intention_speed: 0, - destination: None, - confusion: None, - intention_cell_id: source_cell_id, - tail_intention_cells: vec![], - intermediate_cells: Vec::with_capacity(0), - tail_maneuver: TailIntentionManeuver::default(), - should_stop: false, - }; - - Ok(result) + Ok(create_block_intention(source_cell_id, false)) } #[cfg(test)] diff --git a/src/movement/movement.rs b/src/movement/movement.rs index 106670f..862df48 100644 --- a/src/movement/movement.rs +++ b/src/movement/movement.rs @@ -142,7 +142,7 @@ pub fn movement( net: &GridRoads, vehicles: &mut IndexMap, verbose: &LocalLogger, -) -> Result<(), MovementError> { +) -> Result<(i32, i32), MovementError> { if verbose.is_at_least(VerboseLevel::Main) { verbose.log_with_fields( EVENT_MOVEMENT, @@ -153,6 +153,8 @@ pub fn movement( // Collect vehicles to remove (to avoid borrowing issues during iteration) let mut vehicles_to_remove = Vec::new(); + let mut vehicles_completed = 0i32; + let mut vehicles_lost = 0i32; for (vehicle_id, vehicle) in vehicles.iter_mut() { @@ -173,23 +175,50 @@ pub fn movement( vehicle.apply_intention(); vehicle.is_conflict_participant = false; - // Update bearing only when next cell is different from current + // Update bearing depending on intention maneuver if vehicle.cell_id != vehicle.intention.intention_cell_id { + // vehicle is moving? then set bearing based on actual movement let cell_from = net.get_cell(&vehicle.cell_id) - .ok_or(MovementError::CellNotFound { - cell_id: vehicle.cell_id, - vehicle_id: vehicle.id + .ok_or(MovementError::CellNotFound { + cell_id: vehicle.cell_id, + vehicle_id: vehicle.id })?; let cell_to = net.get_cell(&vehicle.intention.intention_cell_id) - .ok_or(MovementError::CellNotFound { - cell_id: vehicle.intention.intention_cell_id, - vehicle_id: vehicle.id + .ok_or(MovementError::CellNotFound { + cell_id: vehicle.intention.intention_cell_id, + vehicle_id: vehicle.id })?; let pt_from = cell_from.get_point(); let pt_to = cell_to.get_point(); vehicle.bearing = get_bearing(pt_from, pt_to); + } else { + // vehicle is stopped or blocked? bearing = angle to forward direction + use crate::maneuver::LaneChangeType; + match vehicle.intention.intention_maneuver { + LaneChangeType::Block | LaneChangeType::NoChange => { + let current_cell = net.get_cell(&vehicle.cell_id) + .ok_or(MovementError::CellNotFound { + cell_id: vehicle.cell_id, + vehicle_id: vehicle.id + })?; + let forward_id = current_cell.get_forward_id(); + if forward_id > 0 { + if let Some(forward_cell) = net.get_cell(&forward_id) { + let pt_from = current_cell.get_point(); + let pt_to = forward_cell.get_point(); + vehicle.bearing = get_bearing(pt_from, pt_to); + } + } + } + _ => { + // keep bearing as is, dunno if it's good + } + } + } + // Decrement timers only if vehicle moved + if vehicle.cell_id != vehicle.intention.intention_cell_id { // Decrement timers if vehicle.timer_non_acceleration > 0 { vehicle.timer_non_acceleration -= 1; @@ -264,27 +293,28 @@ pub fn movement( vehicle.travel_time += 1; // Check for vehicle removal conditions - if zone_type == ZoneType::Death && vehicle.cell_id != vehicle.destination { - // Vehicle has reached the death zone + if vehicle.cell_id == vehicle.trip_destination { + // Vehicle has reached the destination (priority check) if verbose.is_at_least(VerboseLevel::Main) { verbose.log_with_fields( - EVENT_MOVEMENT_DEAD_END, - "Vehicle done movement due going to dead-end", + EVENT_MOVEMENT_DESTINATION, + "Vehicle done movement due reaching destination", &[("vehicle_id", &vehicle.id)] ); } vehicles_to_remove.push(*vehicle_id); - } - if vehicle.cell_id == vehicle.destination { - // Vehicle has reached the destination + vehicles_completed += 1; + } else if zone_type == ZoneType::Death { + // Vehicle has reached the death zone without reaching destination (lost) if verbose.is_at_least(VerboseLevel::Main) { verbose.log_with_fields( - EVENT_MOVEMENT_DESTINATION, - "Vehicle done movement due reaching destination", + EVENT_MOVEMENT_DEAD_END, + "Vehicle done movement due going to dead-end", &[("vehicle_id", &vehicle.id)] ); } vehicles_to_remove.push(*vehicle_id); + vehicles_lost += 1; } } @@ -295,5 +325,5 @@ pub fn movement( vehicles.swap_remove(&vehicle_id); } - Ok(()) + Ok((vehicles_completed, vehicles_lost)) } \ No newline at end of file diff --git a/src/simulation/session.rs b/src/simulation/session.rs index 84cde8d..e9cf519 100644 --- a/src/simulation/session.rs +++ b/src/simulation/session.rs @@ -140,6 +140,12 @@ pub struct Session { /// Defines the SRID of the world world_srid: SRID, + + /// Cumulative count of vehicles that reached their destination + vehicles_completed: i32, + + /// Cumulative count of vehicles that were lost (reached death zone without reaching destination) + vehicles_lost: i32, } impl Session { @@ -166,6 +172,8 @@ impl Session { _expire_at: 0, steps: 0, world_srid: picked_srid, + vehicles_completed: 0, + vehicles_lost: 0, } } @@ -193,6 +201,8 @@ impl Session { _expire_at: 0, steps: 0, world_srid: picked_srid, + vehicles_completed: 0, + vehicles_lost: 0, } } @@ -555,7 +565,9 @@ impl Session { // 7. Move vehicles let vehicles_grid = self.grids_storage.get_vehicles_net_ref(); - movement(vehicles_grid, &mut self.vehicles, &self.verbose)?; + let (completed, lost) = movement(vehicles_grid, &mut self.vehicles, &self.verbose)?; + self.vehicles_completed += completed; + self.vehicles_lost += lost; // 8. Collect current vehicles positions for state dump let mut states_dump: Vec = Vec::with_capacity(self.vehicles.len()); @@ -591,10 +603,26 @@ impl Session { let timestamp = self.steps; self.steps += 1; + // Log vehicle completion statistics + if self.verbose.is_at_least(VerboseLevel::Main) { + self.verbose.log_with_fields( + EVENT_STEP_COMPLETE, + "Step completed with vehicle statistics", + &[ + ("timestamp", ×tamp), + ("active_vehicles", &self.vehicles.len()), + ("vehicles_completed", &self.vehicles_completed), + ("vehicles_lost", &self.vehicles_lost), + ] + ); + } + Ok(AutomataState { timestamp: timestamp, vehicles: states_dump, tls: tl_states_dump, + vehicles_completed: self.vehicles_completed, + vehicles_lost: self.vehicles_lost, }) } diff --git a/src/simulation/states.rs b/src/simulation/states.rs index 90431f1..24d6f04 100644 --- a/src/simulation/states.rs +++ b/src/simulation/states.rs @@ -24,6 +24,10 @@ pub struct AutomataState { pub vehicles: Vec, /// States of all traffic light groups at this timestamp pub tls: HashMap>, + /// Cumulative count of vehicles that reached their destination + pub vehicles_completed: i32, + /// Cumulative count of vehicles that were lost (reached death zone without reaching destination) + pub vehicles_lost: i32, } /// State of a single vehicle at a specific timestamp diff --git a/src/verbose/verbose.rs b/src/verbose/verbose.rs index 7d65457..4f52040 100644 --- a/src/verbose/verbose.rs +++ b/src/verbose/verbose.rs @@ -109,6 +109,7 @@ pub const EVENT_MOVEMENT_VEHICLE: &str = "movement_vehicle"; pub const EVENT_MOVEMENT_DEAD_END: &str = "movement_dead_end"; pub const EVENT_MOVEMENT_DESTINATION: &str = "movement_destination"; pub const EVENT_ROUTING_STATS: &str = "routing_stats"; +pub const EVENT_STEP_COMPLETE: &str = "step_complete"; pub const EVENT_SESSION_CREATE: &str = "session_create"; pub const EVENT_SESSION_EXPIRED: &str = "session_expired"; pub const EVENT_SESSION_EXTRACT_STATES: &str = "session_extract";