added path finding

game/units/src/components.rs

@@ -26,4 +26,4 @@ pub struct Target(pub HexCoord);
 pub struct Path(pub Vec<Vec3>, pub usize);
 
 #[derive(Component, Debug)]
-pub struct PathTask(pub Task<CommandQueue>);
+pub struct PathTask(pub Task<Option<CommandQueue>>);

game/units/src/nav_data.rs

@@ -1,10 +1,8 @@
+use bevy::prelude::Resource;
 use ordered_float::OrderedFloat;
-use world_generation::{
-	hex_utils::HexCoord,
-	prelude::{Chunk, Map},
-};
+use world_generation::{hex_utils::HexCoord, prelude::Map};
 
-#[derive(Clone)]
+#[derive(Clone, Resource)]
 pub struct NavData {
 	pub tiles: Vec<NavTile>,
 	pub map_height: usize,
@@ -14,12 +12,14 @@ pub struct NavData {
 impl NavData {
 	pub fn get_neighbors(&self, coord: &HexCoord) -> Vec<(HexCoord, OrderedFloat<f32>)> {
 		let mut neighbors = Vec::with_capacity(6);
+		let cur_height = self.get_height(coord);
 		for i in 0..6 {
 			let n = coord.get_neighbor(i);
 			if !self.is_in_bounds(&n) {
 				continue;
 			}
-			neighbors.push((n, OrderedFloat(1.0)));
+			let n_height = self.get_height(&n);
+			neighbors.push((n, OrderedFloat((cur_height - n_height).abs().powi(2))));
 		}
 		return neighbors;
 	}
@@ -27,11 +27,17 @@ impl NavData {
 		return &self.tiles[coord.to_index(self.map_width)];
 	}
 
+	pub fn get_height(&self, coord: &HexCoord) -> f32 {
+		return self.tiles[coord.to_index(self.map_width)].height;
+	}
+
 	pub fn is_in_bounds(&self, pos: &HexCoord) -> bool {
 		return pos.is_in_bounds(self.map_height, self.map_width);
 	}
 
 	pub fn build(map: &Map) -> NavData {
+		#[cfg(feature = "tracing")]
+		let _path_span = info_span!("Build Nav Data").entered();
 		let mut tiles = Vec::with_capacity(map.get_tile_count());
 		let h = map.get_tile_height();
 		let w = map.get_tile_width();
@@ -54,6 +60,25 @@ impl NavData {
 			map_height: h,
 		};
 	}
+
+	pub fn update(&mut self, map: &Map) {
+		#[cfg(feature = "tracing")]
+		let _path_span = info_span!("Update Nav Data").entered();
+		let h = map.get_tile_height();
+		let w = map.get_tile_width();
+		for y in 0..h {
+			for x in 0..w {
+				let coord = HexCoord::from_grid_pos(x, y);
+				let height = map.sample_height(&coord);
+				let tile = NavTile {
+					coord,
+					height,
+					move_cost: 1.0,
+				};
+				self.tiles[y * w + x] = tile;
+			}
+		}
+	}
 }
 
 #[derive(Clone)]
@@ -65,6 +90,6 @@ pub struct NavTile {
 
 impl NavTile {
 	pub fn calculate_heuristic(&self, to: &HexCoord) -> OrderedFloat<f32> {
-		todo!();
+		return (self.coord.distance(to) as f32).into();
 	}
 }

game/units/src/units_debug_plugin.rs

@@ -1,10 +1,7 @@
-use std::f32::consts::E;
-
 use bevy::prelude::*;
 use shared::{resources::TileUnderCursor, sets::GameplaySet, states::AssetLoadState};
-use world_generation::{heightmap, prelude::Map};
 
-use crate::components::{LandUnit, Target, Unit};
+use crate::components::{LandUnit, Path, Target, Unit};
 
 pub struct UnitsDebugPlugin;
 
@@ -13,6 +10,7 @@ impl Plugin for UnitsDebugPlugin {
 		app.add_systems(Update, init.run_if(in_state(AssetLoadState::Loading)));
 
 		app.add_systems(Update, (spawn_test_unit, set_unit_target).in_set(GameplaySet));
+		app.add_systems(FixedUpdate, (visualize_paths).in_set(GameplaySet));
 	}
 }
 
@@ -64,3 +62,21 @@ fn set_unit_target(
 		}
 	}
 }
+
+fn visualize_paths(units: Query<&Path, With<Unit>>, mut gizmos: Gizmos) {
+	for path in units.iter() {
+		if path.1 > path.0.len() {
+			continue;
+		}
+		for node in 1..path.0.len() {
+			let from = path.0[node];
+			let to = path.0[node - 1];
+			let color = if node > path.1 {
+				LinearRgba::rgb(1.0, 0.5, 0.0)
+			} else {
+				LinearRgba::rgb(1.0, 0.5, 1.5)
+			};
+			gizmos.line(from + Vec3::Y * 0.1, to + Vec3::Y * 0.1, color);
+		}
+	}
+}

game/units/src/units_plugin.rs

@@ -8,7 +8,7 @@ use world_generation::{hex_utils::HexCoord, prelude::Map};
 use crate::{
 	assets::unit_asset::UnitAssetPlugin,
 	components::{Path, PathTask, Target, Unit},
-	nav_data::{self, NavData},
+	nav_data::NavData,
 	units_debug_plugin::UnitsDebugPlugin,
 };
 
@@ -65,12 +65,15 @@ fn dispatch_path_requests(
 	map: Res<Map>,
 	mut commands: Commands,
 ) {
+	if units.is_empty() {
+		return;
+	}
 	let mut groups: HashMap<HexCoord, Vec<PathRequest>> = HashMap::new();
 
 	for (transform, target, entity) in units.iter() {
 		let req = PathRequest {
 			entity,
-			to: HexCoord::from_world_pos(transform.translation),
+			from: HexCoord::from_world_pos(transform.translation),
 		};
 		if let Some(group) = groups.get_mut(&target.0) {
 			group.push(req);
@@ -79,45 +82,94 @@ fn dispatch_path_requests(
 		}
 	}
 
+	//todo: only generate when map is changed
 	let nav_data = NavData::build(&map);
 
 	let pool = AsyncComputeTaskPool::get();
-	for (from, units) in groups {
+	for (target, units) in groups {
+		let destinations = get_end_points(&target, units.len(), &map);
+		let mut i = 0;
 		for req in units {
 			let d = nav_data.clone();
+			let dst = destinations[i];
+			i += 1;
 			let task = pool.spawn(async move {
-				let path = calculate_path(&from, &req.to, d);
-				let mut queue = CommandQueue::default();
-				queue.push(move |world: &mut World| {
-					world.entity_mut(req.entity).insert(path);
-				});
-				return queue;
+				#[cfg(feature = "tracing")]
+				let _path_span = info_span!("Path Finding").entered();
+				if let Some(path) = calculate_path(&req.from, &dst, d) {
+					let mut queue = CommandQueue::default();
+					queue.push(move |world: &mut World| {
+						world.entity_mut(req.entity).insert(path);
+					});
+					return Some(queue);
+				}
+				return None;
 			});
-			commands.entity(req.entity).insert(PathTask(task)).remove::<Target>();
+			commands
+				.entity(req.entity)
+				.insert(PathTask(task))
+				.remove::<Target>()
+				.remove::<Path>();
 		}
 	}
 }
 
+fn get_end_points(coord: &HexCoord, count: usize, map: &Map) -> Vec<HexCoord> {
+	let mut result = Vec::with_capacity(count);
+	if count == 1 {
+		return vec![*coord];
+	}
+	result.push(*coord);
+	let mut r = 1;
+	while result.len() < count {
+		let tiles = HexCoord::select_ring(coord, r);
+		let needed = count - result.len();
+		if needed >= tiles.len() {
+			for t in tiles {
+				if map.is_in_bounds(&t) {
+					result.push(t);
+				}
+			}
+		} else {
+			for i in 0..needed {
+				let t = tiles[i];
+				if map.is_in_bounds(&t) {
+					result.push(t);
+				}
+			}
+		}
+		r += 1;
+	}
+
+	return result;
+}
+
 fn resolve_path_task(mut tasks: Query<(&mut PathTask, Entity), With<Unit>>, mut commands: Commands) {
 	for (mut task, entity) in tasks.iter_mut() {
-		if let Some(mut c) = futures::check_ready(&mut task.0) {
-			commands.append(&mut c);
+		if let Some(c) = futures::check_ready(&mut task.0) {
+			if let Some(mut queue) = c {
+				commands.append(&mut queue);
+			}
 			commands.entity(entity).remove::<PathTask>();
 		}
 	}
 }
 
-fn calculate_path(from: &HexCoord, to: &HexCoord, nav: NavData) -> Path {
+fn calculate_path(from: &HexCoord, to: &HexCoord, nav: NavData) -> Option<Path> {
 	let path = astar(
 		from,
 		|n| nav.get_neighbors(n),
 		|n| nav.get(n).calculate_heuristic(to),
 		|n| n == to,
 	);
-	todo!("Convert path");
+	if let Some((nodes, _cost)) = path {
+		let result: Vec<_> = nodes.iter().map(|f| f.to_world(nav.get_height(f))).collect();
+		return Some(Path(result, 1));
+	}
+	return None;
 }
 
 struct PathRequest {
 	pub entity: Entity,
-	pub to: HexCoord,
+	pub from: HexCoord,
 }