Get pathfinding working.
This commit is contained in:
parent
850c5a5cdd
commit
330d915267
|
|
@ -136,10 +136,8 @@ fn snap(
|
|||
snap_timers.insert(entity, SnapTimer::default());
|
||||
transform.rotate(Quat::from_rotation_z(PI));
|
||||
} else if actions.just_pressed(&NavigationAction::SnapCardinal) {
|
||||
println!("Direction: {direction:?}");
|
||||
let yaw: Rot2 = direction.into();
|
||||
let yaw = yaw.as_radians();
|
||||
println!("Yaw: {yaw}");
|
||||
let rotation = Quat::from_rotation_z(yaw);
|
||||
if transform.rotation != rotation {
|
||||
transform.rotation = rotation;
|
||||
|
|
@ -215,7 +213,6 @@ fn controls(
|
|||
let mut speed = **speed;
|
||||
speed *= movement_factor;
|
||||
let move_velocity = direction * speed;
|
||||
// println!("{entity:?}: SetLinearVelocity: {velocity:?}");
|
||||
actions.set_axis_pair(&NavigationAction::SetLinearVelocity, move_velocity);
|
||||
}
|
||||
if velocity.0 != actions.axis_pair(&NavigationAction::SetLinearVelocity) {
|
||||
|
|
@ -224,18 +221,25 @@ fn controls(
|
|||
if actions.axis_pair(&NavigationAction::Translate) != Vec2::ZERO {
|
||||
let pair = actions.axis_pair(&NavigationAction::Translate);
|
||||
let dir = Dir2::new_unchecked(pair.normalize());
|
||||
if spatial_query
|
||||
.cast_shape(
|
||||
let mut can_translate = true;
|
||||
spatial_query.shape_hits_callback(
|
||||
collider,
|
||||
transform.translation.truncate(),
|
||||
transform.yaw().as_radians(),
|
||||
dir,
|
||||
1.,
|
||||
pair.length(),
|
||||
true,
|
||||
SpatialQueryFilter::from_excluded_entities(&sensors),
|
||||
)
|
||||
.is_none()
|
||||
{
|
||||
|hit| {
|
||||
if hit.entity != entity {
|
||||
can_translate = false;
|
||||
false
|
||||
} else {
|
||||
true
|
||||
}
|
||||
},
|
||||
);
|
||||
if can_translate {
|
||||
transform.translation += pair.extend(0.);
|
||||
}
|
||||
actions.set_axis_pair(&NavigationAction::Translate, Vec2::ZERO);
|
||||
|
|
|
|||
|
|
@ -4,8 +4,8 @@ use leafwing_input_manager::{plugin::InputManagerSystem, prelude::*};
|
|||
use pathfinding::prelude::*;
|
||||
|
||||
use crate::{
|
||||
core::{GlobalTransformExt, TransformExt},
|
||||
map::Map,
|
||||
core::GlobalTransformExt,
|
||||
map::{Map, MapObstruction},
|
||||
navigation::{NavigationAction, RotationSpeed, Speed},
|
||||
};
|
||||
|
||||
|
|
@ -65,7 +65,8 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
|
|||
cost *= modifier;
|
||||
}
|
||||
}
|
||||
successors.push((IVec2::new(tile.0 as i32, tile.1 as i32), cost as u32));
|
||||
successors
|
||||
.push((IVec2::new(tile.0 as i32, tile.1 as i32), cost as u32));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -91,11 +92,12 @@ fn calculate_path(
|
|||
),
|
||||
Changed<Destination>,
|
||||
>,
|
||||
obstructions: Query<Entity, With<MapObstruction>>,
|
||||
sensors: Query<Entity, With<Sensor>>,
|
||||
) {
|
||||
for (entity, destination, transform, collider, cost_map, actions) in &mut query {
|
||||
trace!("{entity}: destination: {destination:?}");
|
||||
if transform.translation().truncate().as_ivec2() == **destination {
|
||||
trace!("{entity}: remove1");
|
||||
commands
|
||||
.entity(entity)
|
||||
.remove::<Path>()
|
||||
|
|
@ -103,10 +105,6 @@ fn calculate_path(
|
|||
.remove::<Destination>();
|
||||
continue;
|
||||
}
|
||||
trace!(
|
||||
"{entity:?}: Calculating path from {:?} to {destination:?}",
|
||||
transform.translation().truncate().as_ivec2()
|
||||
);
|
||||
commands.entity(entity).remove::<Path>().remove::<NoPath>();
|
||||
let path = astar(
|
||||
&transform.translation().truncate().as_ivec2(),
|
||||
|
|
@ -126,16 +124,28 @@ fn calculate_path(
|
|||
];
|
||||
for exit in &exits {
|
||||
let mut should_push = true;
|
||||
if !spatial_query
|
||||
.shape_intersections(
|
||||
let mut check = exit.0.as_vec2();
|
||||
if check.x >= 0. {
|
||||
check.x += 0.5;
|
||||
} else {
|
||||
check.x -= 0.5;
|
||||
}
|
||||
if check.y >= 0. {
|
||||
check.y += 0.5;
|
||||
} else {
|
||||
check.y -= 0.5;
|
||||
}
|
||||
let hits = spatial_query.shape_intersections(
|
||||
collider,
|
||||
transform.translation().truncate(),
|
||||
check,
|
||||
transform.yaw().as_radians(),
|
||||
SpatialQueryFilter::default(),
|
||||
)
|
||||
.is_empty()
|
||||
{
|
||||
SpatialQueryFilter::from_excluded_entities(&sensors),
|
||||
);
|
||||
for entity in &hits {
|
||||
if obstructions.contains(*entity) {
|
||||
should_push = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if should_push {
|
||||
let mut cost = exit.1 * 100.;
|
||||
|
|
@ -176,6 +186,7 @@ fn remove_destination(mut commands: Commands, mut removed: RemovedComponents<Des
|
|||
fn negotiate_path(
|
||||
mut commands: Commands,
|
||||
time: Res<Time>,
|
||||
physics_time: Res<Time<Physics>>,
|
||||
spatial_query: SpatialQuery,
|
||||
mut query: Query<(
|
||||
Entity,
|
||||
|
|
@ -183,17 +194,24 @@ fn negotiate_path(
|
|||
&mut ActionState<NavigationAction>,
|
||||
&mut Path,
|
||||
&mut Transform,
|
||||
&GlobalTransform,
|
||||
&Collider,
|
||||
&Speed,
|
||||
Option<&RotationSpeed>,
|
||||
)>,
|
||||
obstructions: Query<Entity, With<MapObstruction>>,
|
||||
sensors: Query<Entity, With<Sensor>>,
|
||||
) {
|
||||
if physics_time.is_paused() {
|
||||
return;
|
||||
}
|
||||
for (
|
||||
entity,
|
||||
actions,
|
||||
mut navigation_actions,
|
||||
mut path,
|
||||
mut transform,
|
||||
global_transform,
|
||||
collider,
|
||||
speed,
|
||||
rotation_speed,
|
||||
|
|
@ -201,17 +219,12 @@ fn negotiate_path(
|
|||
{
|
||||
trace!("{entity:?}: negotiating path");
|
||||
let start = transform.translation.truncate().as_ivec2();
|
||||
trace!(
|
||||
"{entity:?}: start pathfinding from {start:?} to {:?}: at {:?}",
|
||||
path.last(),
|
||||
transform.translation.truncate()
|
||||
);
|
||||
if path.len() > 0 && path[0] == start {
|
||||
trace!("At start, removing");
|
||||
path.remove(0);
|
||||
}
|
||||
if let Some(next) = path.first() {
|
||||
let start = transform.translation.truncate();
|
||||
let start = global_transform.translation().truncate();
|
||||
let mut next = next.as_vec2();
|
||||
if next.x >= 0. {
|
||||
next.x += 0.5;
|
||||
|
|
@ -226,30 +239,30 @@ fn negotiate_path(
|
|||
let mut direction = next - start;
|
||||
direction = direction.normalize();
|
||||
direction *= **speed;
|
||||
direction *= time.delta_seconds();
|
||||
trace!(
|
||||
"{entity:?}: Direction: {direction:?}, Distance: {}",
|
||||
(next - start).length()
|
||||
);
|
||||
if spatial_query
|
||||
.cast_shape(
|
||||
let mut hits = vec![];
|
||||
spatial_query.shape_hits_callback(
|
||||
collider,
|
||||
start,
|
||||
transform.yaw().as_radians(),
|
||||
global_transform.yaw().as_radians(),
|
||||
Dir2::new_unchecked(direction.normalize()),
|
||||
direction.length(),
|
||||
time.delta_seconds(),
|
||||
true,
|
||||
SpatialQueryFilter::default(),
|
||||
)
|
||||
.is_some()
|
||||
{
|
||||
trace!("{entity:?} is stuck");
|
||||
SpatialQueryFilter::from_excluded_entities(&sensors),
|
||||
|hit| {
|
||||
if obstructions.contains(hit.entity) {
|
||||
hits.push(hit.entity);
|
||||
}
|
||||
true
|
||||
},
|
||||
);
|
||||
if !hits.is_empty() {
|
||||
// TODO: Remove when we have an actual character controller.
|
||||
next.x = next.x.trunc();
|
||||
next.y = next.y.trunc();
|
||||
transform.translation = next.extend(0.);
|
||||
} else {
|
||||
navigation_actions.set_axis_pair(&NavigationAction::Translate, direction);
|
||||
let delta = direction * time.delta_seconds();
|
||||
navigation_actions.set_axis_pair(&NavigationAction::Translate, delta);
|
||||
}
|
||||
if rotation_speed.is_some() {
|
||||
let angle = direction.y.atan2(direction.x);
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user