Check chassis footprint and bound pending route exploration (#114)
Reject narrow corridors, recheck motion clearance, and wait for footprint data before flooding the shared query budget. Co-Authored-By: Codex GPT-6 Luna <noreply@openai.com>
This commit is contained in:
@@ -48,7 +48,7 @@ impl Sim {
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
sam.dest = Some(to);
|
sam.dest = Some(to);
|
||||||
let route = route::Route::new(sam.pos, to, slack, sam.kind.climbs(), self.tick);
|
let route = route::Route::new(sam.pos, to, slack, sam.kind, self.tick);
|
||||||
self.routes.map.insert(sam.id, route);
|
self.routes.map.insert(sam.id, route);
|
||||||
sam.target = None;
|
sam.target = None;
|
||||||
sam.gun_target = None;
|
sam.gun_target = None;
|
||||||
@@ -102,7 +102,7 @@ impl Sim {
|
|||||||
sam.march = sam.kind.march_ticks();
|
sam.march = sam.kind.march_ticks();
|
||||||
continue;
|
continue;
|
||||||
};
|
};
|
||||||
let route = routes.map.entry(sam.id).or_insert_with(|| route::Route::new(sam.pos, to, 0.0, sam.kind.climbs(), now));
|
let route = routes.map.entry(sam.id).or_insert_with(|| route::Route::new(sam.pos, to, 0.0, sam.kind, now));
|
||||||
let (go, last) = match route.drive(ground, now, sam.pos, share) {
|
let (go, last) = match route.drive(ground, now, sam.pos, share) {
|
||||||
route::Drive::Wait(why) => {
|
route::Drive::Wait(why) => {
|
||||||
if route.waited(now) > 40 && !route.told {
|
if route.waited(now) > 40 && !route.told {
|
||||||
@@ -145,7 +145,8 @@ impl Sim {
|
|||||||
let turning = angle_diff(sam.heading, heading_of(flat)).abs();
|
let turning = angle_diff(sam.heading, heading_of(flat)).abs();
|
||||||
let step = SPEED * sam.drive_speed() * (1.0 - turning / std::f64::consts::PI).max(0.2);
|
let step = SPEED * sam.drive_speed() * (1.0 - turning / std::f64::consts::PI).max(0.2);
|
||||||
let next = sam.pos + ahead * step;
|
let next = sam.pos + ahead * step;
|
||||||
// The column ahead: a wall or a drop the grid missed turns the vehicle back to the planner.
|
// The column and chassis clearance ahead: a wall, low ceiling or a change after planning
|
||||||
|
// turns the vehicle back to the planner. Unknown voxels remain a wait, never a drive.
|
||||||
let Some(y) = ground.at(next, now) else {
|
let Some(y) = ground.at(next, now) else {
|
||||||
if route.wait_ahead() {
|
if route.wait_ahead() {
|
||||||
out.messages.push(format!("{} №{}: {}", sam.kind.name(), n + 1, route::Why::NoMap.text()));
|
out.messages.push(format!("{} №{}: {}", sam.kind.name(), n + 1, route::Why::NoMap.text()));
|
||||||
@@ -156,6 +157,23 @@ impl Sim {
|
|||||||
}
|
}
|
||||||
continue;
|
continue;
|
||||||
};
|
};
|
||||||
|
match route.clearance(ground, next, now) {
|
||||||
|
route::Clearance::Clear => {}
|
||||||
|
route::Clearance::Pending => {
|
||||||
|
if route.wait_ahead() {
|
||||||
|
out.messages.push(format!("{} №{}: {}", sam.kind.name(), n + 1, route::Why::NoMap.text()));
|
||||||
|
routes.map.remove(&sam.id);
|
||||||
|
sam.dest = None;
|
||||||
|
sam.state = SamState::Deploying;
|
||||||
|
sam.march = sam.kind.march_ticks();
|
||||||
|
}
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
route::Clearance::Blocked => {
|
||||||
|
route.blocked_ahead(next, sam.pos);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
route.ahead_ok();
|
route.ahead_ok();
|
||||||
let rise = y - sam.pos.y;
|
let rise = y - sam.pos.y;
|
||||||
if rise > route::STEP_UP || rise < -route::STEP_DOWN {
|
if rise > route::STEP_UP || rise < -route::STEP_DOWN {
|
||||||
|
|||||||
+113
-7
@@ -64,6 +64,44 @@ pub(super) enum Drive {
|
|||||||
Fail(Why),
|
Fail(Why),
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Result of checking the vehicle's clearance at its next position.
|
||||||
|
pub(super) enum Clearance {
|
||||||
|
Clear,
|
||||||
|
Blocked,
|
||||||
|
Pending,
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Conservative chassis envelope. The circular footprint is the circumscribed plan-view of the
|
||||||
|
/// folded vehicle; it makes turns safe without adding heading to the bounded A* state.
|
||||||
|
#[derive(Clone, Copy)]
|
||||||
|
struct Chassis {
|
||||||
|
radius: f64,
|
||||||
|
height: f64,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Chassis {
|
||||||
|
fn for_kind(kind: SamKind) -> Self {
|
||||||
|
let (radius, height) = match kind.spec().family {
|
||||||
|
Family::Trainable => (2.25, 3.0),
|
||||||
|
Family::Gun => (1.75, 3.0),
|
||||||
|
Family::Turret => (2.0, 3.25),
|
||||||
|
Family::Vertical => (2.75, 3.5),
|
||||||
|
Family::Static => (2.0, 3.0),
|
||||||
|
Family::Truck => (1.75, 2.75),
|
||||||
|
Family::Light => (1.25, 2.5),
|
||||||
|
};
|
||||||
|
Self { radius, height }
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Integer voxel sample points inside the conservative footprint.
|
||||||
|
fn samples(self) -> Vec<(i32, i32)> {
|
||||||
|
let r = self.radius.ceil() as i32;
|
||||||
|
(-r..=r).flat_map(|x| (-r..=r).filter_map(move |z| {
|
||||||
|
((x * x + z * z) as f64 <= self.radius * self.radius + 0.5).then_some((x, z))
|
||||||
|
})).collect()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#[derive(Clone, Copy)]
|
#[derive(Clone, Copy)]
|
||||||
struct Info {
|
struct Info {
|
||||||
h: f32,
|
h: f32,
|
||||||
@@ -103,6 +141,7 @@ pub(super) struct Route {
|
|||||||
slack: f64,
|
slack: f64,
|
||||||
cell: i32,
|
cell: i32,
|
||||||
slope: f64,
|
slope: f64,
|
||||||
|
chassis: Chassis,
|
||||||
start: (i32, i32),
|
start: (i32, i32),
|
||||||
lo: (i32, i32),
|
lo: (i32, i32),
|
||||||
hi: (i32, i32),
|
hi: (i32, i32),
|
||||||
@@ -131,14 +170,15 @@ pub(super) struct Route {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl Route {
|
impl Route {
|
||||||
pub(super) fn new(pos: DVec3, dest: DVec3, slack: f64, slope: f64, now: u64) -> Self {
|
pub(super) fn new(pos: DVec3, dest: DVec3, slack: f64, kind: SamKind, now: u64) -> Self {
|
||||||
let dist = horizontal(dest - pos).length();
|
let dist = horizontal(dest - pos).length();
|
||||||
let cell = if dist <= 400.0 { 4 } else if dist <= 900.0 { 6 } else { 8 };
|
let cell = if dist <= 400.0 { 4 } else if dist <= 900.0 { 6 } else { 8 };
|
||||||
let mut r = Self {
|
let mut r = Self {
|
||||||
dest,
|
dest,
|
||||||
slack,
|
slack,
|
||||||
cell,
|
cell,
|
||||||
slope,
|
slope: kind.climbs(),
|
||||||
|
chassis: Chassis::for_kind(kind),
|
||||||
start: (0, 0),
|
start: (0, 0),
|
||||||
lo: (0, 0),
|
lo: (0, 0),
|
||||||
hi: (0, 0),
|
hi: (0, 0),
|
||||||
@@ -232,10 +272,50 @@ impl Route {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
if verdict.ok {
|
||||||
|
match self.clearance_at(ground, DVec3::new(f64::from(x) + 0.5, f64::from(verdict.h), f64::from(z) + 0.5), now) {
|
||||||
|
Clearance::Clear => {}
|
||||||
|
Clearance::Blocked => {
|
||||||
|
let blocked = Info { h: verdict.h, ok: false };
|
||||||
|
self.cells.insert(c, blocked);
|
||||||
|
return Eval::Bad;
|
||||||
|
}
|
||||||
|
Clearance::Pending => return Eval::Pending,
|
||||||
|
}
|
||||||
|
}
|
||||||
self.cells.insert(c, verdict);
|
self.cells.insert(c, verdict);
|
||||||
if verdict.ok { Eval::Ok(verdict.h) } else { Eval::Bad }
|
if verdict.ok { Eval::Ok(verdict.h) } else { Eval::Bad }
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Check a chassis-sized footprint against sampled support columns and voxel headroom.
|
||||||
|
/// Unknown columns or sections are pending so neither search nor motion assumes they are open.
|
||||||
|
fn clearance_at(&self, ground: &mut Ground, pos: DVec3, now: u64) -> Clearance {
|
||||||
|
let base = pos.y;
|
||||||
|
let mut pending = false;
|
||||||
|
for (dx, dz) in self.chassis.samples() {
|
||||||
|
let x = pos.x.floor() as i32 + dx;
|
||||||
|
let z = pos.z.floor() as i32 + dz;
|
||||||
|
let Some(column) = ground.column(x, z, 1024) else { pending = true; continue };
|
||||||
|
let Some(h) = column else { return Clearance::Blocked };
|
||||||
|
let dh = h - base;
|
||||||
|
if dh > STEP_UP || dh < -STEP_DOWN {
|
||||||
|
return Clearance::Blocked;
|
||||||
|
}
|
||||||
|
let top = DVec3::new(f64::from(x) + 0.5, h + 0.05, f64::from(z) + 0.5);
|
||||||
|
match ground.world.sweep(top, top + DVec3::Y * self.chassis.height, false, now) {
|
||||||
|
world::Sweep::Clear => {}
|
||||||
|
world::Sweep::Unknown { pending: true, .. } | world::Sweep::Wait { .. } => pending = true,
|
||||||
|
world::Sweep::Unknown { .. } | world::Sweep::Hit { .. } => return Clearance::Blocked,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if pending { Clearance::Pending } else { Clearance::Clear }
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Recheck the envelope at the live position, since obstacles may change after planning.
|
||||||
|
pub(super) fn clearance(&self, ground: &mut Ground, pos: DVec3, now: u64) -> Clearance {
|
||||||
|
self.clearance_at(ground, pos, now)
|
||||||
|
}
|
||||||
|
|
||||||
/// Relaxes the edge from the opened cell `c` to `nb`; parks it when the land between isn't known yet.
|
/// Relaxes the edge from the opened cell `c` to `nb`; parks it when the land between isn't known yet.
|
||||||
fn relax(&mut self, ground: &mut Ground, now: u64, c: (i32, i32), nb: (i32, i32)) {
|
fn relax(&mut self, ground: &mut Ground, now: u64, c: (i32, i32), nb: (i32, i32)) {
|
||||||
if nb.0 < self.lo.0 || nb.1 < self.lo.1 || nb.0 > self.hi.0 || nb.1 > self.hi.1 {
|
if nb.0 < self.lo.0 || nb.1 < self.lo.1 || nb.0 > self.hi.0 || nb.1 > self.hi.1 {
|
||||||
@@ -305,7 +385,7 @@ impl Route {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
if now >= self.retry_at && !self.parked.is_empty() {
|
if now >= self.retry_at && !self.parked.is_empty() {
|
||||||
self.retry_at = now + 6;
|
self.retry_at = now + 1;
|
||||||
let parked = std::mem::take(&mut self.parked);
|
let parked = std::mem::take(&mut self.parked);
|
||||||
for (k, &(c, nb)) in parked.iter().enumerate() {
|
for (k, &(c, nb)) in parked.iter().enumerate() {
|
||||||
// The game loads the chunk of the first few, if it loads on request.
|
// The game loads the chunk of the first few, if it loads on request.
|
||||||
@@ -336,6 +416,9 @@ impl Route {
|
|||||||
for (di, dj) in [(1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)] {
|
for (di, dj) in [(1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)] {
|
||||||
self.relax(ground, now, c, (c.0 + di, c.1 + dj));
|
self.relax(ground, now, c, (c.0 + di, c.1 + dj));
|
||||||
}
|
}
|
||||||
|
// Let pending footprint queries settle before opening a broad, less useful frontier.
|
||||||
|
// Otherwise three trucks can saturate the shared world budget without ever moving.
|
||||||
|
if !self.parked.is_empty() { break; }
|
||||||
if self.expanded >= MAX_NODES {
|
if self.expanded >= MAX_NODES {
|
||||||
self.settle(now);
|
self.settle(now);
|
||||||
return;
|
return;
|
||||||
@@ -505,13 +588,13 @@ mod tests {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Drives a Patriot from the origin to `to` over land of the given heights (water: a height over a lower floor).
|
/// Drives a Patriot from the origin to `to` over land of the given heights (water: a height over a lower floor).
|
||||||
fn march<H: Fn(i32, i32) -> i32, B: Fn(i32, i32) -> i32>(height: H, floor: B, to: DVec3, ticks: u32) -> (Sim, Out, u32) {
|
fn march_kind<H: Fn(i32, i32) -> i32, B: Fn(i32, i32) -> i32>(kind: SamKind, height: H, floor: B, to: DVec3, ticks: u32) -> (Sim, Out, u32) {
|
||||||
let src = Src { height, floor };
|
let src = Src { height, floor };
|
||||||
let mut sim = Sim::new(3);
|
let mut sim = Sim::new(3);
|
||||||
sim.damage = false;
|
sim.damage = false;
|
||||||
let mut ground = Ground::default();
|
let mut ground = Ground::default();
|
||||||
let id = sim.id();
|
let id = sim.id();
|
||||||
sim.sams.push(Sam::new(id, SamKind::Patriot, DVec3::new(0.5, 64.0, 0.5), 0.0));
|
sim.sams.push(Sam::new(id, kind, DVec3::new(0.5, 64.0, 0.5), 0.0));
|
||||||
let mut out = Out::default();
|
let mut out = Out::default();
|
||||||
for _ in 0..3 {
|
for _ in 0..3 {
|
||||||
sim.tick += 1;
|
sim.tick += 1;
|
||||||
@@ -538,6 +621,10 @@ mod tests {
|
|||||||
(sim, out, used)
|
(sim, out, used)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
fn march<H: Fn(i32, i32) -> i32, B: Fn(i32, i32) -> i32>(height: H, floor: B, to: DVec3, ticks: u32) -> (Sim, Out, u32) {
|
||||||
|
march_kind(SamKind::Patriot, height, floor, to, ticks)
|
||||||
|
}
|
||||||
|
|
||||||
// A wall across the way, with a gap far to one side: the vehicle goes round and arrives.
|
// A wall across the way, with a gap far to one side: the vehicle goes round and arrives.
|
||||||
#[test]
|
#[test]
|
||||||
fn a_wall_with_a_gap_is_driven_round() {
|
fn a_wall_with_a_gap_is_driven_round() {
|
||||||
@@ -572,8 +659,27 @@ mod tests {
|
|||||||
// Open ground: straight on, arrives.
|
// Open ground: straight on, arrives.
|
||||||
#[test]
|
#[test]
|
||||||
fn open_ground_is_driven_straight() {
|
fn open_ground_is_driven_straight() {
|
||||||
let (sim, _, _) = march(|_, _| 64, |_, _| 64, DVec3::new(100.0, 64.0, 30.5), 1500);
|
let (sim, out, _) = march(|_, _| 64, |_, _| 64, DVec3::new(100.0, 64.0, 30.5), 1500);
|
||||||
let s = &sim.sams[0];
|
let s = &sim.sams[0];
|
||||||
assert!(!s.moving() && s.pos.distance(DVec3::new(100.0, 64.0, 30.5)) < 3.0, "{:?}", s.pos);
|
let route = sim.routes.map.get(&s.id).map(|r| (r.expanded, r.parked.len(), r.open.len(), r.cells.len()));
|
||||||
|
assert!(!s.moving() && s.pos.distance(DVec3::new(100.0, 64.0, 30.5)) < 3.0, "pos={:?} state={:?} messages={:?} route={route:?}", s.pos, s.state, out.messages);
|
||||||
|
}
|
||||||
|
|
||||||
|
// A point-column route fits through this one-block slit, but the S-400 chassis does not.
|
||||||
|
#[test]
|
||||||
|
fn s400_rejects_a_one_block_corridor() {
|
||||||
|
let walls = |x: i32, z: i32| if (-500..500).contains(&x) && z != 0 { 96 } else { 64 };
|
||||||
|
let (sim, out, _) = march_kind(SamKind::S400, walls, walls, DVec3::new(60.5, 64.0, 0.5), 4000);
|
||||||
|
assert!(!sim.sams[0].moving(), "{:?}", sim.sams[0].state);
|
||||||
|
assert!(sim.sams[0].pos.x < 20.0, "narrow passage was traversed: {:?}", sim.sams[0].pos);
|
||||||
|
assert!(out.messages.iter().any(|m| m.contains("прохода нет") || m.contains("не проехать")), "{:?}", out.messages);
|
||||||
|
}
|
||||||
|
|
||||||
|
// A broad, level lane remains driveable for the same large vehicle.
|
||||||
|
#[test]
|
||||||
|
fn s400_uses_a_wide_corridor() {
|
||||||
|
let walls = |x: i32, z: i32| if (-500..500).contains(&x) && !(-6..=6).contains(&z) { 96 } else { 64 };
|
||||||
|
let (sim, _, _) = march_kind(SamKind::S400, walls, walls, DVec3::new(60.5, 64.0, 0.5), 5000);
|
||||||
|
assert!(!sim.sams[0].moving() && sim.sams[0].pos.distance(DVec3::new(60.5, 64.0, 0.5)) < 3.0, "pos={:?} state={:?}", sim.sams[0].pos, sim.sams[0].state);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user