From 47e413c530d226ab7467c5a82f167b610740d3fa Mon Sep 17 00:00:00 2001 From: Joshua Barretto Date: Tue, 7 Jul 2020 18:23:01 +0100 Subject: [PATCH] Improved pathfinding tolerance and reliability --- common/src/path.rs | 210 ++++++++++++++++++++++++++-------------- common/src/sys/agent.rs | 2 +- 2 files changed, 136 insertions(+), 76 deletions(-) diff --git a/common/src/path.rs b/common/src/path.rs index 8388edb36c..b51bea7cb3 100644 --- a/common/src/path.rs +++ b/common/src/path.rs @@ -75,86 +75,124 @@ impl Route { where V: BaseVol + ReadVol, { - let next0 = self - .next(0) - .unwrap_or_else(|| pos.map(|e| e.floor() as i32)); - let next1 = self.next(1).unwrap_or(next0); - if vol.get(next0).map(|b| b.is_solid()).unwrap_or(false) { - None - } else { - let next_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0); - if pos.xy().distance_squared(next_tgt.xy()) < traversal_tolerance.powf(2.0) - && next_tgt.z - pos.z < 0.2 - && next_tgt.z - pos.z > -2.2 + let (next0, next1, next_tgt) = loop { + let next0 = self + .next(0) + .unwrap_or_else(|| pos.map(|e| e.floor() as i32)); + + // Stop using obstructed paths + if vol.get(next0).map(|b| b.is_solid()).unwrap_or(false) { + return None; + } + + let next1 = self.next(1).unwrap_or(next0); + let next0_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0); + let next1_tgt = next1.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0); + + // We might be able to skip a node in some cases to avoid doubling-back + let closest_tgt = if next0_tgt.distance_squared(pos) < next1_tgt.distance_squared(pos) { + next0_tgt + } else { + next1_tgt + }; + + // Determine whether we're close enough to the next to to consider it completed + if pos.xy().distance_squared(closest_tgt.xy()) < traversal_tolerance.powf(2.0) + && closest_tgt.z - pos.z < 0.2 + && closest_tgt.z - pos.z > -2.2 && vel.z <= 0.0 + // Only consider the node reached if there's nothing solid between us and it && vol - .ray(pos + Vec3::unit_z() * 0.5, next_tgt + Vec3::unit_z() * 0.5) + .ray(pos + Vec3::unit_z() * 1.5, closest_tgt + Vec3::unit_z() * 1.5) .until(|block| block.is_solid()) .cast() .0 - > pos.distance(next_tgt) * 0.9 + > pos.distance(closest_tgt) * 0.9 + && self.next_idx < self.path.len() { + // Node completed, move on to the next one self.next_idx += 1; + } else { + // The next node hasn't been reached yet, use it as a target + break (next0, next1, next0_tgt); } + }; - let line = LineSegment2 { - start: pos.xy(), - end: pos.xy() + vel.xy() * 100.0, - }; + let line = LineSegment2 { + start: pos.xy(), + end: pos.xy() + vel.xy() * 100.0, + }; - let align = |block_pos: Vec3| { - (0..2) - .map(|i| (0..2).map(move |j| Vec2::new(i, j))) - .flatten() - .map(|rpos| block_pos + rpos) - .map(|block_pos| { - let block_posf = block_pos.xy().map(|e| e as f32); - let proj = line.projected_point(block_posf); - let clamped = proj.clamped( - block_pos.xy().map(|e| e as f32), - block_pos.xy().map(|e| e as f32), - ); + // We don't always want to aim for the centre of block since this can create + // jerky zig-zag movement. This function attempts to find a position + // inside a target block's area that aligned nicely with our velocity. + // This has a twofold benefit: + // + // 1. Entities can move at any angle when + // running on a flat surface + // + // 2. We don't have to search diagonals when + // pathfinding - cartesian positions are enough since this code will + // make the entity move smoothly along them + let align = |block_pos: Vec3| { + (0..2) + .map(|i| (0..2).map(move |j| Vec2::new(i, j))) + .flatten() + .map(|rpos| block_pos + rpos) + .map(|block_pos| { + let block_posf = block_pos.xy().map(|e| e as f32); + let proj = line.projected_point(block_posf); + let clamped = proj.clamped( + block_pos.xy().map(|e| e as f32), + block_pos.xy().map(|e| e as f32), + ); - (proj.distance_squared(clamped), clamped) - }) - .min_by_key(|(d2, _)| (d2 * 1000.0) as i32) - .unwrap() - .1 - }; + (proj.distance_squared(clamped), clamped) + }) + .min_by_key(|(d2, _)| (d2 * 1000.0) as i32) + .unwrap() + .1 + }; - let cb = CubicBezier2 { - start: pos.xy(), - ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or_else(Vec2::zero), - ctrl1: align(next0), - end: align(next1), - }; + let cb = CubicBezier2 { + start: pos.xy(), + ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or_else(Vec2::zero) * 1.25, + ctrl1: align(next0), + end: align(next1), + }; - let tgt2d = cb.evaluate(0.5); - let tgt = Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z; - let tgt_dir = (tgt - pos) - .xy() - .try_normalized() - .unwrap_or_else(Vec2::unit_y); - let next_dir = cb - .evaluate_derivative(0.5) - .try_normalized() - .unwrap_or(tgt_dir); + // Use a cubic spline of the next few targets to come up with a sensible target + // position. We want to use a position that gives smooth movement but is + // also accurate enough to avoid the agent getting stuck under ledges or + // falling off walls. + let tgt2d = cb.evaluate(0.5); + let tgt = Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z; + let tgt_dir = (tgt - pos) + .xy() + .try_normalized() + .unwrap_or_else(Vec2::unit_y); + let next_dir = cb + .evaluate_derivative(0.5) + .try_normalized() + .unwrap_or(tgt_dir); - //let vel_dir = vel.xy().try_normalized().unwrap_or(Vec2::zero()); - //let avg_dir = (tgt_dir * 0.2 + vel_dir * - // 0.8).try_normalized().unwrap_or(Vec2::zero()); let bearing = - // Vec3::::from(avg_dir * (tgt - pos).xy().magnitude()) + Vec3::unit_z() * - // (tgt.z - pos.z); + //let vel_dir = vel.xy().try_normalized().unwrap_or(Vec2::zero()); + //let avg_dir = (tgt_dir * 0.2 + vel_dir * + // 0.8).try_normalized().unwrap_or(Vec2::zero()); let bearing = + // Vec3::::from(avg_dir * (tgt - pos).xy().magnitude()) + Vec3::unit_z() * + // (tgt.z - pos.z); - Some(( - tgt - pos, - next_dir - .dot(vel.xy().try_normalized().unwrap_or_else(Vec2::zero)) - .max(0.0) - * 0.75 - + 0.25, - )) - } + Some(( + tgt - pos, + // Control the entity's speed to hopefully stop us falling off walls on sharp corners. + // This code is very imperfect: it does its best but it can still fail for particularly + // fast entities. + next_dir + .dot(vel.xy().try_normalized().unwrap_or_else(Vec2::zero)) + .max(0.0) + * 0.75 + + 0.25, + )) } } @@ -186,33 +224,53 @@ impl Chaser { { let pos_to_tgt = pos.distance(tgt); + // If we're already close to the target then there's nothing to do if ((pos - tgt) * Vec3::new(1.0, 1.0, 2.0)).magnitude_squared() < min_dist.powf(2.0) { + self.route = None; return None; } let bearing = if let Some(end) = self.route.as_ref().and_then(|r| r.path().end().copied()) { let end_to_tgt = end.map(|e| e as f32).distance(tgt); - if end_to_tgt > pos_to_tgt * 0.3 + 5.0 || thread_rng().gen::() < 0.005 { + // If the target has moved significantly since the path was generated then it's + // time to search for a new path. Also, do this randomly from time + // to time to avoid any edge cases that cause us to get stuck. In + // theory this shouldn't happen, but in practice the world is full + // of unpredictable obstacles that are more than willing to mess up + // our day. TODO: Come up with a better heuristic for this + if end_to_tgt > pos_to_tgt * 0.3 + 5.0 + /* || thread_rng().gen::() < 0.005 */ + { None } else { self.route .as_mut() .and_then(|r| r.traverse(vol, pos, vel, traversal_tolerance)) + // In theory this filter isn't needed, but in practice agents often try to take + // stale paths that start elsewhere. This code makes sure that we're only using + // paths that start near us, avoiding the agent doubling back to chase a stale + // path. + .filter(|(bearing, _)| bearing.xy() + .magnitude_squared() < (traversal_tolerance * 3.0).powf(2.0)) } } else { None }; - // TODO: What happens when we get stuck? if let Some(bearing) = bearing { Some(bearing) } else { + // Only search for a path if the target has moved from their last position. We + // don't want to be thrashing the pathfinding code for targets that + // we're unable to access! if self .last_search_tgt .map(|last_tgt| last_tgt.distance(tgt) > pos_to_tgt * 0.15 + 5.0) .unwrap_or(true) + || self.route.is_none() { let (start_pos, path) = find_path(&mut self.astar, vol, pos, tgt); + // Don't use a stale path if start_pos.distance_squared(pos) < 4.0f32.powf(2.0) { self.route = path.map(Route::from); } else { @@ -331,14 +389,14 @@ where .unwrap_or(true))) }) .map(move |(pos, dir)| pos + dir) - .chain( - DIAGONALS - .iter() - .filter(move |(dir, [a, b])| { - is_walkable(&(pos + *dir)) && walkable[*a] && walkable[*b] - }) - .map(move |(dir, _)| pos + *dir), - ) + // .chain( + // DIAGONALS + // .iter() + // .filter(move |(dir, [a, b])| { + // is_walkable(&(pos + *dir)) && walkable[*a] && + // walkable[*b] }) + // .map(move |(dir, _)| pos + *dir), + // ) }; let crow_line = LineSegment2 { @@ -347,6 +405,8 @@ where }; let transition = |a: &Vec3, b: &Vec3| { + // Modify the heuristic a little in order to prefer paths that take us on a + // straight line toward our target. This means we get smoother movement. 1.0 + crow_line.distance_to_point(b.xy().map(|e| e as f32)) * 0.025 + (b.z - a.z - 1).max(0) as f32 * 3.0 }; diff --git a/common/src/sys/agent.rs b/common/src/sys/agent.rs index 5fcfd5d47a..8e96fa54e9 100644 --- a/common/src/sys/agent.rs +++ b/common/src/sys/agent.rs @@ -126,7 +126,7 @@ impl<'a> System<'a> for Sys { // and so can afford to be less precise when trying to move around // the world (especially since they would otherwise get stuck on // obstacles that smaller entities would not). - let traversal_tolerance = scale + vel.0.magnitude() * 0.3; + let traversal_tolerance = scale + vel.0.magnitude() * 0.25; let mut do_idle = false; let mut choose_target = false;