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
V: BaseVol<Vox = Block> + 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<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),
);
// 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<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)
})
.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::<f32>::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::<f32>::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::<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
} 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<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
+ (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
// 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;