Mirrored_Repos/veloren
1
0
Fork 0
mirror of https://gitlab.com/veloren/veloren.git synced 2024-08-30 18:12:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Improved pathfinding tolerance and reliability

Browse Source
This commit is contained in:
Joshua Barretto 2020-07-07 18:23:01 +01:00
parent 23c774c8da
commit 47e413c530
2 changed files with 136 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

210
common/src/path.rs
View File

@ -75,86 +75,124 @@ impl Route {
where where
V: BaseVol<Vox = Block> + ReadVol, V: BaseVol<Vox = Block> + ReadVol,
{ {
let next0 = self let (next0, next1, next_tgt) = loop {
.next(0) let next0 = self
.unwrap_or_else(|| pos.map(|e| e.floor() as i32)); .next(0)
let next1 = self.next(1).unwrap_or(next0); .unwrap_or_else(|| pos.map(|e| e.floor() as i32));
if vol.get(next0).map(|b| b.is_solid()).unwrap_or(false) {
None // Stop using obstructed paths
} else { if vol.get(next0).map(|b| b.is_solid()).unwrap_or(false) {
let next_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0); return None;
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 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 && vel.z <= 0.0
// Only consider the node reached if there's nothing solid between us and it
&& vol && 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()) .until(|block| block.is_solid())
.cast() .cast()
.0 .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; 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 { let line = LineSegment2 {
start: pos.xy(), start: pos.xy(),
end: pos.xy() + vel.xy() * 100.0, end: pos.xy() + vel.xy() * 100.0,
}; };
let align = |block_pos: Vec3<i32>| { // We don't always want to aim for the centre of block since this can create
(0..2) // jerky zig-zag movement. This function attempts to find a position
.map(|i| (0..2).map(move |j| Vec2::new(i, j))) // inside a target block's area that aligned nicely with our velocity.
.flatten() // This has a twofold benefit:
.map(|rpos| block_pos + rpos) //
.map(|block_pos| { // 1. Entities can move at any angle when
let block_posf = block_pos.xy().map(|e| e as f32); // running on a flat surface
let proj = line.projected_point(block_posf); //
let clamped = proj.clamped( // 2. We don't have to search diagonals when
block_pos.xy().map(|e| e as f32), // pathfinding - cartesian positions are enough since this code will
block_pos.xy().map(|e| e as f32), // make the entity move smoothly along them
); let align = |block_pos: Vec3<i32>| {
(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) (proj.distance_squared(clamped), clamped)
}) })
.min_by_key(|(d2, _)| (d2 * 1000.0) as i32) .min_by_key(|(d2, _)| (d2 * 1000.0) as i32)
.unwrap() .unwrap()
.1 .1
}; };
let cb = CubicBezier2 { let cb = CubicBezier2 {
start: pos.xy(), start: pos.xy(),
ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or_else(Vec2::zero), ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or_else(Vec2::zero) * 1.25,
ctrl1: align(next0), ctrl1: align(next0),
end: align(next1), end: align(next1),
}; };
let tgt2d = cb.evaluate(0.5); // Use a cubic spline of the next few targets to come up with a sensible target
let tgt = Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z; // position. We want to use a position that gives smooth movement but is
let tgt_dir = (tgt - pos) // also accurate enough to avoid the agent getting stuck under ledges or
.xy() // falling off walls.
.try_normalized() let tgt2d = cb.evaluate(0.5);
.unwrap_or_else(Vec2::unit_y); let tgt = Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z;
let next_dir = cb let tgt_dir = (tgt - pos)
.evaluate_derivative(0.5) .xy()
.try_normalized() .try_normalized()
.unwrap_or(tgt_dir); .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 vel_dir = vel.xy().try_normalized().unwrap_or(Vec2::zero());
//let avg_dir = (tgt_dir * 0.2 + vel_dir * //let avg_dir = (tgt_dir * 0.2 + vel_dir *
// 0.8).try_normalized().unwrap_or(Vec2::zero()); let bearing = // 0.8).try_normalized().unwrap_or(Vec2::zero()); let bearing =
// Vec3::<f32>::from(avg_dir * (tgt - pos).xy().magnitude()) + Vec3::unit_z() * // Vec3::<f32>::from(avg_dir * (tgt - pos).xy().magnitude()) + Vec3::unit_z() *
// (tgt.z - pos.z); // (tgt.z - pos.z);
Some(( Some((
tgt - pos, tgt - pos,
next_dir // Control the entity's speed to hopefully stop us falling off walls on sharp corners.
.dot(vel.xy().try_normalized().unwrap_or_else(Vec2::zero)) // This code is very imperfect: it does its best but it can still fail for particularly
.max(0.0) // fast entities.
* 0.75 next_dir
+ 0.25, .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); 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) { if ((pos - tgt) * Vec3::new(1.0, 1.0, 2.0)).magnitude_squared() < min_dist.powf(2.0) {
self.route = None;
return None; return None;
} }
let bearing = if let Some(end) = self.route.as_ref().and_then(|r| r.path().end().copied()) { 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); 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::<f32>() < 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::<f32>() < 0.005 */
{
None None
} else { } else {
self.route self.route
.as_mut() .as_mut()
.and_then(|r| r.traverse(vol, pos, vel, traversal_tolerance)) .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 { } else {
None None
}; };
// TODO: What happens when we get stuck?
if let Some(bearing) = bearing { if let Some(bearing) = bearing {
Some(bearing) Some(bearing)
} else { } 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 if self
.last_search_tgt .last_search_tgt
.map(|last_tgt| last_tgt.distance(tgt) > pos_to_tgt * 0.15 + 5.0) .map(|last_tgt| last_tgt.distance(tgt) > pos_to_tgt * 0.15 + 5.0)
.unwrap_or(true) .unwrap_or(true)
|| self.route.is_none()
{ {
let (start_pos, path) = find_path(&mut self.astar, vol, pos, tgt); 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) { if start_pos.distance_squared(pos) < 4.0f32.powf(2.0) {
self.route = path.map(Route::from); self.route = path.map(Route::from);
} else { } else {
@ -331,14 +389,14 @@ where
.unwrap_or(true))) .unwrap_or(true)))
}) })
.map(move |(pos, dir)| pos + dir) .map(move |(pos, dir)| pos + dir)
.chain( // .chain(
DIAGONALS // DIAGONALS
.iter() // .iter()
.filter(move |(dir, [a, b])| { // .filter(move |(dir, [a, b])| {
is_walkable(&(pos + *dir)) && walkable[*a] && walkable[*b] // is_walkable(&(pos + *dir)) && walkable[*a] &&
}) // walkable[*b] })
.map(move |(dir, _)| pos + *dir), // .map(move |(dir, _)| pos + *dir),
) // )
}; };
let crow_line = LineSegment2 { let crow_line = LineSegment2 {
@ -347,6 +405,8 @@ where
}; };
let transition = |a: &Vec3<i32>, b: &Vec3<i32>| { let transition = |a: &Vec3<i32>, b: &Vec3<i32>| {
// 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 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 + (b.z - a.z - 1).max(0) as f32 * 3.0
}; };

2
common/src/sys/agent.rs
View File

@ -126,7 +126,7 @@ impl<'a> System<'a> for Sys {
// and so can afford to be less precise when trying to move around // and so can afford to be less precise when trying to move around
// the world (especially since they would otherwise get stuck on // the world (especially since they would otherwise get stuck on
// obstacles that smaller entities would not). // 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 do_idle = false;
let mut choose_target = false; let mut choose_target = false;