From 8bfed5566103d0538497a2dd625441d5587584aa Mon Sep 17 00:00:00 2001 From: Waridley Date: Sat, 6 Jan 2024 16:19:07 -0600 Subject: [PATCH] Drop petgraph, custom AStar impl --- rs/Cargo.lock | 1 - rs/Cargo.toml | 4 +- rs/src/nav.rs | 476 +++++++++++++-------------------------- rs/src/nav/alg.rs | 219 ++++++++++++++++++ rs/src/nav/heightmap.rs | 381 +++++++++++++++++++++++++++++++ rs/src/planet.rs | 16 +- rs/src/planet/chunks.rs | 47 +++- rs/src/planet/terrain.rs | 33 +-- rs/src/util.rs | 3 +- 9 files changed, 828 insertions(+), 352 deletions(-) create mode 100644 rs/src/nav/alg.rs create mode 100644 rs/src/nav/heightmap.rs diff --git a/rs/Cargo.lock b/rs/Cargo.lock index 257970a..535c61e 100644 --- a/rs/Cargo.lock +++ b/rs/Cargo.lock @@ -4161,7 +4161,6 @@ dependencies = [ "noise", "num-traits", "ordered-float", - "petgraph", "rapier3d", "ron", "serde", diff --git a/rs/Cargo.toml b/rs/Cargo.toml index a9c41b0..6858dec 100644 --- a/rs/Cargo.toml +++ b/rs/Cargo.toml @@ -78,13 +78,12 @@ bevy_common_assets = { version = "0.8.0", features = ["ron"] } array-init = "2.1.0" bevy_pkv = "0.9.0" # settings crossbeam = "0.8.2" -fixedbitset = "0.4.2" # petgraph edge storage +fixedbitset = "0.4.2" # navigation graph edge storage futures-lite = "2.1.0" # especially for `yield_now()` nanorand = { version = "0.7.0", default-features = false, features = ["std", "wyrand", "getrandom"] } # wasm is broken without std and getrandom noise = "0.8.2" num-traits = "0.2.17" ordered-float = "4.2.0" -petgraph = { version = "0.6.4", default-features = false } # pathfinding ron = "0.8.1" static_assertions = "1.1.0" serde = "1" @@ -118,7 +117,6 @@ nanorand = { workspace = true } noise = { workspace = true } num-traits = { workspace = true } ordered-float = { workspace = true } -petgraph = { workspace = true } ron = { workspace = true } static_assertions = { workspace = true } serde = { workspace = true } diff --git a/rs/src/nav.rs b/rs/src/nav.rs index e8a3114..b3b1eb9 100644 --- a/rs/src/nav.rs +++ b/rs/src/nav.rs @@ -1,359 +1,197 @@ -use bevy::prelude::*; -use fixedbitset::FixedBitSet; -use petgraph::visit::{ - Data, EdgeRef, GraphBase, GraphRef, IntoEdgeReferences, IntoEdges, IntoNeighbors, Visitable, +use crate::{ + nav::{alg::AStar, heightmap::TriId}, + planet::chunks::ChunkIndex, + util::Todo, }; +use bevy::{prelude::*, utils::HashMap}; use rapier3d::prelude::*; -use std::{f32::consts::SQRT_2, ops::Range}; +use std::{collections::HashSet, sync::Weak}; -pub struct NavGraph { - _chunks: Vec, - _seams: Vec, -} +pub mod alg; +pub mod heightmap; -pub struct NavMesh { - _vertices: Vec, - _triangle_indices: Vec<[usize; 3]>, -} - -pub struct NavVolume { - _shape: TypedShape<'static>, +#[derive(Resource, Debug)] +pub struct NavGraph { + heightmaps: HashMap>, + structures: HashMap>, + islands: HashMap>, + heightmap_structure_seams: HashMap, // should probably be bi-directional map + island_structure_seams: HashMap, // should probably be bi-directional map } -pub enum NavChunk { - Mesh(NavMesh), - Volume(NavVolume), +/// ID of a triangle in the world by chunk index and triangle index within that chunk. +#[derive(Default, Copy, Clone, Debug, PartialEq, Eq, Hash)] +pub struct PlanetTri { + pub chunk: ChunkIndex, + pub tri: TriId, } -pub struct Seam {} - -#[derive(Copy, Clone, Deref)] -pub struct HeightmapNavGraph<'a, F: FnMut(TriPair) -> bool + Copy> { +#[derive(Copy, Clone, Debug, Deref)] +pub struct NavGraphRef<'a, EdgeGen> { #[deref] - heights: &'a HeightField, - filter: F, -} - -impl<'a, F: FnMut(TriPair) -> bool + Copy> HeightmapNavGraph<'a, F> { - pub fn new(heights: &'a HeightField, filter: F) -> Self { - Self { heights, filter } - } - - pub fn astar( - self, - start: ::NodeId, - goal: ::NodeId, - edge_cost: impl FnMut(TriPair) -> f32, - ) -> Option<(f32, Vec<::NodeId>)> { - petgraph::algo::astar( - self, - start, - |id| id == goal, - edge_cost, - |id| { - // // Manhattan distance - // let (i, j, left) = self.split_triangle_id(id); - // let (gi, gj, gleft) = self.split_triangle_id(goal); - // let left = !left as usize; // treat left as 0 and right as 1 - // let gleft = !gleft as usize; - // ((gi * 2 + gleft).abs_diff(i * 2 + left) - // + (gj * 2 + gleft).abs_diff(j * 2 + left)) - // as f32 - - // Euclidian distance - let goal = self.triangle_at_id(goal).unwrap().center(); - let center = self.triangle_at_id(id).unwrap().center(); - Vec3::from(goal).distance(Vec3::from(center)) - }, - ) - } -} - -pub fn height_field_graph_with_max_climb( - heights: &HeightField, - max_climb_radians: f32, -) -> HeightmapNavGraph bool + Copy + '_> { - HeightmapNavGraph { - heights, - filter: move |pair: TriPair| { - let (si, sj, sl) = heights.split_triangle_id(pair.source()); - let (ti, tj, _tl) = heights.split_triangle_id(pair.target()); - let (si, sj, ti, tj) = (si as f32, sj as f32, ti as f32, tj as f32); - let dir = if si == ti && sj == tj { - // same cell, towards other triangle - if sl { - Vec3::new(SQRT_2, 0.0, SQRT_2) - } else { - Vec3::new(-SQRT_2, 0.0, -SQRT_2) - } - } else { - Vec3::new(tj - sj, 0.0, ti - si).normalize() - }; - let snorm = heights - .triangle_at_id(pair.source()) - .unwrap() - .normal() - .unwrap(); - if snorm.angle(&Vec3::Y.into()) > max_climb_radians { - // Already on a steep slope - let dot = dir.dot(snorm.into()); - if dot < 0.0 { - // Against normal -- Uphill - false - } else { - // With normal -- Downhill - true - } - } else { - // Not on steep slope, check slope of target - let tnorm = heights - .triangle_at_id(pair.target()) - .unwrap() - .normal() - .unwrap(); - let dot = dir.dot(tnorm.into()); - if dot < 0.0 { - // Against normal, so uphill - tnorm.angle(&Vec3::Y.into()) <= max_climb_radians - } else { - // Normal is pointing away from us, so downhill - true - } - } - }, - } + graph: &'a NavGraph, + edge_generator: EdgeGen, } -pub trait FnsThatShouldBePub { - fn num_triangles(&self) -> usize; - - fn split_triangle_id(&self, id: u32) -> (usize, usize, bool); - - fn triangle_id(&self, i: usize, j: usize, left: bool) -> u32; -} - -impl FnsThatShouldBePub for HeightField { - fn num_triangles(&self) -> usize { - self.nrows() * self.ncols() * 2 - } - - fn split_triangle_id(&self, id: u32) -> (usize, usize, bool) { - let left = id < self.num_triangles() as u32 / 2; - let cell_id = if left { - id as usize - } else { - id as usize - self.num_triangles() / 2 - }; - let j = cell_id / self.nrows(); - let i = cell_id - j * self.nrows(); - (i, j, left) - } - - fn triangle_id(&self, i: usize, j: usize, left: bool) -> u32 { - let tid = j * self.nrows() + i; - if left { - tid as u32 - } else { - (tid + self.num_triangles() / 2) as u32 +impl<'a, EdgeGen: EdgeVendor> NavGraphRef<'a, EdgeGen> { + pub fn new(graph: &'a NavGraph, edge_generator: EdgeGen) -> Self { + Self { + graph, + edge_generator, } } } -impl bool + Copy> GraphBase for HeightmapNavGraph<'_, F> { - type EdgeId = TriPair; - type NodeId = TriId; -} - -impl bool + Copy> Visitable for HeightmapNavGraph<'_, F> { - type Map = FixedBitSet; - - fn visit_map(&self) -> Self::Map { - FixedBitSet::with_capacity(self.heights.nrows() * self.heights.ncols()) - } - - fn reset_map(&self, map: &mut Self::Map) { - if map.len() != self.heights.nrows() * self.heights.nrows() { - *map = FixedBitSet::with_capacity(self.heights.nrows() * self.heights.ncols()) - } else { - map.clear() - } +impl<'a, EdgeGen: EdgeVendor + Copy + 'static> NavGraphRef<'a, EdgeGen> { + fn edges(self, a: NavIdx) -> impl Iterator + 'a + 'static { + let Self { + graph, + edge_generator, + .. + } = self; + let iter = edge_generator.find_edges(graph, a); + iter + } + + pub fn neighbors(self, a: NavIdx) -> impl Iterator + 'a + 'static { + let Self { + graph, + edge_generator, + } = self; + let iter = edge_generator.find_edges(graph, a); + iter.map(move |edge| match edge { + NavEdge::GroundToGround { target, .. } => NavIdx::Ground(target), + NavEdge::StructureToStructure { target, .. } => NavIdx::Structure(target), + NavEdge::GroundToStructure { target, .. } => NavIdx::Structure(target), + NavEdge::StructureToGround { target, .. } => NavIdx::Ground(target), + }) + } + + // PERF: may want custom set of FixedBitSets for different nodes, rather than huge HashSet + pub fn visit_map(self: &Self) -> HashSet { + HashSet::new() + } + + pub fn reset_map(self: &Self, map: &mut HashSet) { + map.clear() + } +} + +// impl<'a, EdgGen: Copy> IntoEdgeReferences for NavGraphRef<'a, EdgGen> { +// type EdgeRef = NavEdge; +// type EdgeReferences = EdgeRefs; +// +// fn edge_references(self) -> Self::EdgeReferences { +// EdgeRefs +// } +// } +// +// pub struct EdgeRefs; +// +// impl Iterator for EdgeRefs { +// type Item = NavEdge; +// +// fn next(&mut self) -> Option { +// todo!() +// } +// } + +impl<'a, EdgeGen: EdgeVendor + Copy + 'static> NavGraphRef<'a, EdgeGen> { + pub async fn astar( + self, + start: NavIdx, + goal: NavIdx, + edge_cost: impl FnMut(NavEdge) -> f32, + ) -> Option<(f32, Vec)> { + AStar::new(self.edge_generator, edge_cost).run(self.graph, start, goal) } } -impl<'a, F: FnMut(TriPair) -> bool + Copy> IntoEdgeReferences for HeightmapNavGraph<'a, F> { - type EdgeRef = TriPair; - type EdgeReferences = EdgeIter<'a, F>; - - fn edge_references(self) -> Self::EdgeReferences { - EdgeIter::new(self) - } +pub struct NavMesh { + _vertices: Vec, + _triangle_indices: Vec<[usize; 3]>, } -pub struct EdgeIter<'a, F: FnMut(TriPair) -> bool + Copy> { - graph: HeightmapNavGraph<'a, F>, - range: Range, - curr_edges: Option< as IntoIterator>::IntoIter>, +pub struct NavVolume { + _shape: TypedShape<'static>, } -impl<'a, F: FnMut(TriPair) -> bool + Copy> EdgeIter<'a, F> { - fn new(data: HeightmapNavGraph<'a, F>) -> Self { - let range = 0..data.num_triangles() as u32; - Self { - graph: data, - range, - curr_edges: None, - } - } +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +pub enum NavIdx { + Ground(PlanetTri), + Structure(Todo), } -impl bool + Copy> Iterator for EdgeIter<'_, F> { - type Item = TriPair; - - fn next(&mut self) -> Option { - loop { - if let Some(mut curr) = self.curr_edges.take() { - if let Some(pair) = curr.next() { - return Some(pair); - } else { - self.curr_edges = self.range.next().map(|id| self.graph.edges(id)); - } - } else { - return None; - } - } - } +/// A trait for functions that provide edges for `NavGraph::into_edges` without creating work for +/// the pathfinding algorithm for edges that a given entity will never be able to traverse. +/// +/// This is basically a trait alias for a complicated `FnMut` bound. +pub trait EdgeVendor { + fn find_edges<'a>( + self, + graph: &'a NavGraph, + node: NavIdx, + ) -> impl Iterator + 'a + 'static; } -impl bool + Copy> Data for HeightmapNavGraph<'_, F> { - type NodeWeight = Vec3; // normal - type EdgeWeight = (); +pub struct SumEdges { + pub a: A, + pub b: B, } -impl bool + Copy> GraphRef for HeightmapNavGraph<'_, F> {} - -impl bool + Copy> IntoNeighbors for HeightmapNavGraph<'_, F> { - type Neighbors = as IntoIterator>::IntoIter; - - fn neighbors(mut self, a: Self::NodeId) -> Self::Neighbors { - //TODO: Create special iterator instead of allocating vecs - let (i, j, left) = self.split_triangle_id(a); - let mut neighbors = Vec::new(); - - let mut try_push = |this: &mut Self, target| { - if (this.filter)(TriPair::new(a, target)) { - neighbors.push(target); - } - }; - - if left { - let right = self.triangle_id(i, j, false); - (try_push)(&mut self, right); - - if i > 0 { - let up = self.triangle_id(i - 1, j, false); - (try_push)(&mut self, up); - } - - if j > 0 { - let left = self.triangle_id(i, j - 1, false); - (try_push)(&mut self, left); - } - } else { - let left = self.triangle_id(i, j, true); - (try_push)(&mut self, left); - - if j < self.heights.ncols() - 1 { - let right = self.triangle_id(i, j + 1, true); - (try_push)(&mut self, right); - } - - if i < self.heights.nrows() - 1 { - let down = self.triangle_id(i + 1, j, true); - (try_push)(&mut self, down); - } - } - neighbors.into_iter() +impl EdgeVendor for SumEdges { + fn find_edges<'a>( + self, + graph: &'a NavGraph, + node: NavIdx, + ) -> impl Iterator + 'a + 'static { + self.a + .find_edges(graph, node) + .chain(self.b.find_edges(graph, node)) } } -impl bool + Copy> IntoEdges for HeightmapNavGraph<'_, F> { - type Edges = as IntoIterator>::IntoIter; - - fn edges(mut self, a: Self::NodeId) -> Self::Edges { - //TODO: Create special iterator instead of allocating vecs - let mut edges = Vec::new(); - let (i, j, left) = self.split_triangle_id(a); - - let mut try_push = |this: &mut Self, target| { - let pair = TriPair::new(a, target); - if (this.filter)(pair) { - edges.push(pair); - } - }; - - if left { - let right = self.triangle_id(i, j, false); - (try_push)(&mut self, right); - - if i > 0 { - let up = self.triangle_id(i - 1, j, false); - (try_push)(&mut self, up); - } - - if j > 0 { - let left = self.triangle_id(i, j - 1, false); - (try_push)(&mut self, left); - } - } else { - let left = self.triangle_id(i, j, true); - (try_push)(&mut self, left); - - if j < self.heights.ncols() - 1 { - let right = self.triangle_id(i, j + 1, true); - (try_push)(&mut self, right); - } +pub trait CombineVendors: EdgeVendor { + fn plus(self, next: Other) -> impl EdgeVendor; +} - if i < self.heights.nrows() - 1 { - let down = self.triangle_id(i + 1, j, true); - (try_push)(&mut self, down); - } - } - edges.into_iter() +impl CombineVendors for T { + fn plus(self, next: Other) -> impl EdgeVendor { + SumEdges { a: self, b: next } } } -type TriId = u32; - #[derive(Copy, Clone, Debug, PartialEq)] -pub struct TriPair { - source: TriId, - target: TriId, -} - -impl TriPair { - fn new(source: TriId, target: TriId) -> Self { - Self { source, target } +pub enum NavEdge { + GroundToGround { + source: PlanetTri, + target: PlanetTri, + }, + StructureToStructure { + source: Todo, + target: Todo, + }, + GroundToStructure { + source: PlanetTri, + target: Todo, + }, + StructureToGround { + source: Todo, + target: PlanetTri, + }, +} + +impl NavEdge { + pub fn target(self) -> NavIdx { + match self { + NavEdge::GroundToGround { target, .. } => NavIdx::Ground(target), + NavEdge::StructureToStructure { target, .. } => NavIdx::Structure(target), + NavEdge::GroundToStructure { target, .. } => NavIdx::Structure(target), + NavEdge::StructureToGround { target, .. } => NavIdx::Ground(target), + } } } -impl EdgeRef for TriPair { - type NodeId = TriId; - type EdgeId = Self; - type Weight = (); - - fn source(&self) -> Self::NodeId { - self.source - } - - fn target(&self) -> Self::NodeId { - self.target - } - - fn weight(&self) -> &Self::Weight { - &() - } - - fn id(&self) -> Self::EdgeId { - *self - } +pub trait NavEdgeFilter { + fn test(&mut self, edge: NavEdge) -> bool; } diff --git a/rs/src/nav/alg.rs b/rs/src/nav/alg.rs new file mode 100644 index 0000000..1cdac97 --- /dev/null +++ b/rs/src/nav/alg.rs @@ -0,0 +1,219 @@ +use bevy::{math::Vec3, prelude::default}; +use std::{ + cmp::Ordering, + collections::{ + hash_map::Entry::{Occupied, Vacant}, + BinaryHeap, HashMap, + }, + sync::Weak, +}; + +use crate::nav::{ + EdgeVendor, NavEdge, NavGraph, NavGraphRef, NavIdx, + NavIdx::{Ground, Structure}, +}; + +#[derive(Default)] +pub struct PathTracker { + pub came_from: HashMap, +} + +impl PathTracker { + pub fn new() -> PathTracker { + PathTracker { + came_from: HashMap::new(), + } + } + + fn set_predecessor(&mut self, node: NavIdx, previous: NavIdx) { + self.came_from.insert(node, previous); + } + + fn reconstruct_path_to(&self, last: NavIdx) -> Vec { + let mut path = vec![last]; + + let mut current = last; + while let Some(&previous) = self.came_from.get(¤t) { + path.push(previous); + current = previous; + } + + path.reverse(); + + path + } +} + +#[derive(Default)] +pub struct AStar +where + EdgeGen: EdgeVendor + Copy + 'static, + Cost: FnMut(NavEdge) -> f32, +{ + edge_generator: EdgeGen, + visit_next: BinaryHeap, + scores: HashMap, + estimate_scores: HashMap, + path_tracker: PathTracker, + edge_cost: Cost, +} + +impl AStar +where + EdgeGen: EdgeVendor + Copy + 'static, + Cost: FnMut(NavEdge) -> f32, +{ + pub fn new(edge_generator: EdgeGen, edge_cost: Cost) -> Self { + Self { + edge_generator, + visit_next: default(), + scores: default(), + estimate_scores: default(), + path_tracker: default(), + edge_cost, + } + } + pub fn run( + &mut self, + graph: &NavGraph, + start: NavIdx, + goal: NavIdx, + ) -> Option<(f32, Vec)> + where + Cost: FnMut(NavEdge) -> f32, + { + let Self { + edge_generator, + visit_next, + scores, + estimate_scores, + path_tracker, + edge_cost, + } = self; + + let graph = NavGraphRef { + graph, + edge_generator: *edge_generator, + }; + + let estimate_cost = |id| match (id, goal) { + (Ground(start), Ground(end)) => { + let Some((start_hm, end_hm)) = graph + .heightmaps + .get(&start.chunk) + .and_then(Weak::upgrade) + .zip(graph.heightmaps.get(&end.chunk).and_then(Weak::upgrade)) + else { + return f32::INFINITY; + }; + let Some((start, end)) = start_hm + .triangle_at_id(start.tri) + .zip(end_hm.triangle_at_id(end.tri)) + else { + return f32::INFINITY; + }; + Vec3::from(start.center()).distance(Vec3::from(end.center())) + } + (Ground(_tri), Structure(_)) => todo!(), + (Structure(_), Ground(_tri)) => todo!(), + (Structure(_), Structure(_)) => todo!(), + }; + + path_tracker.came_from.clear(); + scores.insert(start, 0.0); + visit_next.push(MinScored(estimate_cost(start), start)); + + while let Some(MinScored(estimate_score, node)) = visit_next.pop() { + if node == goal { + let path = path_tracker.reconstruct_path_to(node); + let cost = scores[&node]; + return Some((cost, path)); + } + + // This lookup can be unwrapped without fear of panic since the node was necessarily scored + // before adding it to `visit_next`. + let node_score = scores[&node]; + + match estimate_scores.entry(node) { + Occupied(mut entry) => { + // If the node has already been visited with an equal or lower score than now, then + // we do not need to re-visit it. + if *entry.get() <= estimate_score { + continue; + } + entry.insert(estimate_score); + } + Vacant(entry) => { + entry.insert(estimate_score); + } + } + + for edge in graph.edges(node) { + let next = edge.target(); + let next_score = node_score + edge_cost(edge); + + match scores.entry(next) { + Occupied(mut entry) => { + // No need to add neighbors that we have already reached through a shorter path + // than now. + if *entry.get() <= next_score { + continue; + } + entry.insert(next_score); + } + Vacant(entry) => { + entry.insert(next_score); + } + } + + path_tracker.set_predecessor(next, node); + let next_estimate_score = next_score + estimate_cost(next); + visit_next.push(MinScored(next_estimate_score, next)); + } + } + + None + } +} + +#[derive(Copy, Clone, Debug)] +pub struct MinScored(pub f32, pub NavIdx); + +impl PartialEq for MinScored { + #[inline] + fn eq(&self, other: &MinScored) -> bool { + self.cmp(other) == Ordering::Equal + } +} + +impl Eq for MinScored {} + +impl PartialOrd for MinScored { + #[inline] + fn partial_cmp(&self, other: &MinScored) -> Option { + Some(self.cmp(other)) + } +} + +impl Ord for MinScored { + #[inline] + fn cmp(&self, other: &MinScored) -> Ordering { + let a = &self.0; + let b = &other.0; + if a == b { + Ordering::Equal + } else if a < b { + Ordering::Greater + } else if a > b { + Ordering::Less + } else if a.ne(a) && b.ne(b) { + // these are the NaN cases + Ordering::Equal + } else if a.ne(a) { + // Order NaN less, so that it is last in the MinScore order + Ordering::Less + } else { + Ordering::Greater + } + } +} diff --git a/rs/src/nav/heightmap.rs b/rs/src/nav/heightmap.rs new file mode 100644 index 0000000..39a295a --- /dev/null +++ b/rs/src/nav/heightmap.rs @@ -0,0 +1,381 @@ +use super::{EdgeVendor, NavEdge, NavEdgeFilter, NavGraph, NavIdx, PlanetTri}; +use crate::planet::chunks::{ChunkIndex, CHUNK_COLS, CHUNK_ROWS}; +use bevy::{prelude::*, utils::HashMap}; +use rapier3d::geometry::HeightField; +use std::{f32::consts::SQRT_2, sync::Weak}; + +pub trait FnsThatShouldBePub { + fn num_triangles(&self) -> usize; + + fn split_triangle_id(&self, id: u32) -> (usize, usize, bool); + + fn triangle_id(&self, i: usize, j: usize, left: bool) -> u32; +} + +impl FnsThatShouldBePub for HeightField { + fn num_triangles(&self) -> usize { + num_tris(self.nrows(), self.ncols()) + } + + fn split_triangle_id(&self, id: u32) -> (usize, usize, bool) { + split_triangle_id(self.nrows(), self.ncols(), id) + } + + fn triangle_id(&self, i: usize, j: usize, left: bool) -> u32 { + tri_id(self.nrows(), self.ncols(), i, j, left) + } +} + +/// Triangle index within a specific chunk's `HeightField`. +pub type TriId = u32; + +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct TriPair { + pub source: TriId, + pub target: TriId, +} + +impl TriPair { + pub fn new(source: TriId, target: TriId) -> Self { + Self { source, target } + } +} + +pub const fn num_tris(nrows: usize, ncols: usize) -> usize { + nrows * ncols * 2 +} + +pub const fn split_triangle_id(nrows: usize, ncols: usize, id: TriId) -> (usize, usize, bool) { + let num_tris = num_tris(nrows, ncols); + let left = id < num_tris as u32 / 2; + let cell_id = if left { + id as usize + } else { + id as usize - (nrows * ncols) + }; + let j = cell_id / nrows; + let i = cell_id - j * nrows; + (i, j, left) +} + +pub const fn tri_id(nrows: usize, ncols: usize, i: usize, j: usize, left: bool) -> TriId { + let tid = j * nrows + i; + if left { + tid as u32 + } else { + (tid + num_tris(nrows, ncols) / 2) as u32 + } +} + +#[derive(Debug, Copy, Clone)] +pub struct GroundEdges { + filter: F, +} + +impl EdgeVendor for GroundEdges { + fn find_edges<'a>( + self, + graph: &'a NavGraph, + node: NavIdx, + ) -> impl Iterator + 'static { + match node { + NavIdx::Ground(id) => graph + .heightmaps + .get(&id.chunk) + .and_then(Weak::upgrade) + .map_or_else( + || GroundTriEdgeIter::none(self.filter.clone()), + |heights| { + GroundTriEdgeIter::new( + heights.nrows(), + heights.ncols(), + id, + self.filter.clone(), + ) + }, + ), + _ => { + bevy::log::error!("I think this should be unreachable, might want to handle this case explicitly if not"); + GroundTriEdgeIter::none(self.filter.clone()) + } + } + } +} + +pub struct GroundTriEdgeIter { + nrows: usize, + ncols: usize, + source: PlanetTri, + filter: F, + curr: TriNeighbor, +} + +impl GroundTriEdgeIter { + fn new(nrows: usize, ncols: usize, source: PlanetTri, filter: F) -> Self { + Self { + nrows, + ncols, + source, + filter, + curr: TriNeighbor::A, + } + } + fn none(filter: F) -> Self { + Self { + nrows: 0, + ncols: 0, + source: default(), + filter, + curr: TriNeighbor::Done, + } + } + + fn num_tris(&self) -> usize { + num_tris(self.nrows, self.ncols) + } + + fn split_tri_id(&self) -> (usize, usize, bool) { + split_triangle_id(self.nrows, self.ncols, self.source.tri) + } + + fn tri_id(&self, i: usize, j: usize, left: bool) -> u32 { + let tid = j * self.nrows + i; + if left { + tid as u32 + } else { + (tid + self.num_tris() / 2) as u32 + } + } +} + +impl Iterator for GroundTriEdgeIter { + type Item = NavEdge; + + fn next(&mut self) -> Option { + use TriNeighbor::*; + let source = self.source; + let chunk = source.chunk; + let (i, j, left) = self.split_tri_id(); + loop { + match (self.curr, left) { + (A, left) => { + self.curr = B; + + // Same square, other triangle + let a = NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk, + tri: self.tri_id(i, j, !left), + }, + }; + + if self.filter.test(a) { + return Some(a); + } + } + (B, true) => { + self.curr = C; + + let up = if i > 0 { + NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk, + tri: self.tri_id(i - 1, j, false), + }, + } + } else { + NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk: ChunkIndex { + x: chunk.x, + y: chunk.y - 1, + }, + tri: self.tri_id(self.nrows - 1, j, false), + }, + } + }; + + if self.filter.test(up) { + return Some(up); + } + } + (B, false) => { + self.curr = C; + + let right = if j < self.ncols - 1 { + NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk, + tri: self.tri_id(i, j + 1, true), + }, + } + } else { + NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk: ChunkIndex { + x: chunk.x + 1, + y: chunk.y, + }, + tri: self.tri_id(i, 0, true), + }, + } + }; + + if self.filter.test(right) { + return Some(right); + } + } + (C, true) => { + self.curr = Done; + + let left = if j > 0 { + NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk, + tri: self.tri_id(i, j - 1, false), + }, + } + } else { + NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk: ChunkIndex { + x: chunk.x - 1, + y: chunk.y, + }, + tri: self.tri_id(i, self.ncols - 1, false), + }, + } + }; + + if self.filter.test(left) { + return Some(left); + } + } + (C, false) => { + self.curr = Done; + + let down = if i < self.nrows - 1 { + NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk, + tri: self.tri_id(i + 1, j, true), + }, + } + } else { + NavEdge::GroundToGround { + source, + target: PlanetTri { + chunk: ChunkIndex { + x: chunk.x + 1, + y: chunk.y, + }, + tri: self.tri_id(i, 0, true), + }, + } + }; + + if self.filter.test(down) { + return Some(down); + } + } + (Done, _) => return None, + } + } + } +} + +pub struct AllGroundTris; + +impl NavEdgeFilter for AllGroundTris { + fn test(&mut self, edge: NavEdge) -> bool { + matches!(edge, NavEdge::GroundToGround { .. }) + } +} + +pub struct MaxClimb<'heights> { + pub chunks: &'heights HashMap>, + pub radians: f32, +} + +impl NavEdgeFilter for MaxClimb<'_> { + fn test(&mut self, edge: NavEdge) -> bool { + let NavEdge::GroundToGround { source, target } = edge else { + return false; + }; + + let (si, sj, sl) = split_triangle_id(CHUNK_ROWS, CHUNK_COLS, source.tri); + let (ti, tj, _tl) = split_triangle_id(CHUNK_ROWS, CHUNK_COLS, target.tri); + let (si, sj, ti, tj) = (si as f32, sj as f32, ti as f32, tj as f32); + let dir = if si == ti && sj == tj { + // same cell, towards other triangle + if sl { + Vec3::new(SQRT_2, 0.0, SQRT_2) + } else { + Vec3::new(-SQRT_2, 0.0, -SQRT_2) + } + } else { + // towards closest triangle in adjacent cell + Vec3::new(tj - sj, 0.0, ti - si).normalize() + }; + let Some(snorm) = self + .chunks + .get(&source.chunk) + .and_then(Weak::upgrade) + .and_then(|heights| heights.triangle_at_id(source.tri)) + .and_then(|tri| tri.normal()) + .as_deref() + .copied() + else { + return false; + }; + if snorm.angle(&Vec3::Y.into()) > self.radians { + // Already on a steep slope + let dot = dir.dot(snorm.into()); + if dot < 0.0 { + // Against normal -- Uphill + false + } else { + // With normal -- Downhill + true + } + } else { + // Not on steep slope, check slope of target + let Some(tnorm) = self + .chunks + .get(&target.chunk) + .and_then(Weak::upgrade) + .and_then(|heights| heights.triangle_at_id(source.tri)) + .and_then(|tri| tri.normal()) + .as_deref() + .copied() + else { + return false; + }; + let dot = dir.dot(tnorm.into()); + if dot < 0.0 { + // Against normal, so uphill + tnorm.angle(&Vec3::Y.into()) <= self.radians + } else { + // Normal is pointing away from us, so downhill + true + } + } + } +} + +#[derive(Default, Copy, Clone, Debug)] +enum TriNeighbor { + #[default] + A, + B, + C, + Done, +} diff --git a/rs/src/planet.rs b/rs/src/planet.rs index 627edb7..22c2b15 100644 --- a/rs/src/planet.rs +++ b/rs/src/planet.rs @@ -1,6 +1,6 @@ use crate::{planet::day_night::DayNightCycle, util::IntoFnPlugin}; use bevy::prelude::*; -use bevy_rapier3d::{na::Vector2, parry::math::Vector}; +use bevy_rapier3d::na::{Vector2, Vector3}; use serde::{Deserialize, Serialize}; use std::ops::{Add, Sub}; @@ -15,14 +15,12 @@ pub fn plugin(app: &mut App) -> &mut App { .register_type::() } -#[derive( - Component, Default, Debug, Copy, Clone, Deref, DerefMut, PartialEq, Serialize, Deserialize, -)] -pub struct PlanetVec3(pub Vector); +#[derive(Default, Debug, Copy, Clone, Deref, DerefMut, PartialEq, Serialize, Deserialize)] +pub struct PlanetVec3(pub Vector3); impl PlanetVec3 { pub fn new(x: f64, y: f64, z: f64) -> Self { - Self(Vector::new(x, y, z)) + Self(Vector3::new(x, y, z)) } pub fn relative_to(self, other: Self) -> Vec3 { @@ -33,7 +31,7 @@ impl PlanetVec3 { impl From for PlanetVec3 { fn from(value: Vec3) -> Self { - Self(Vector::new(value.x as f64, value.y as f64, value.z as f64)) + Self(Vector3::new(value.x as f64, value.y as f64, value.z as f64)) } } @@ -59,7 +57,7 @@ impl Add for PlanetVec3 { type Output = Self; fn add(self, rhs: Vec3) -> Self::Output { - let rhs = Vector::new(rhs.x as f64, rhs.y as f64, rhs.z as f64); + let rhs = Vector3::new(rhs.x as f64, rhs.y as f64, rhs.z as f64); Self(*self + rhs) } } @@ -76,7 +74,7 @@ impl Sub for PlanetVec3 { type Output = Self; fn sub(self, rhs: Vec3) -> Self::Output { - let rhs = Vector::new(rhs.x as f64, rhs.y as f64, rhs.z as f64); + let rhs = Vector3::new(rhs.x as f64, rhs.y as f64, rhs.z as f64); Self(*self - rhs) } } diff --git a/rs/src/planet/chunks.rs b/rs/src/planet/chunks.rs index ba00443..4bbdda5 100644 --- a/rs/src/planet/chunks.rs +++ b/rs/src/planet/chunks.rs @@ -1,9 +1,50 @@ use super::terrain::Ground; -use crate::planet::PlanetVec3; -use bevy::prelude::*; +use crate::planet::PlanetVec2; +use bevy::{prelude::*, utils::HashMap}; + +pub const CHUNK_SIZE: f32 = 128.0; +pub const CHUNK_ROWS: usize = CHUNK_SIZE as usize; +pub const CHUNK_COLS: usize = CHUNK_SIZE as usize; #[derive(Bundle, Debug)] pub struct ChunkBundle { - pub center: PlanetVec3, + pub center: ChunkCenter, + pub index: ChunkIndex, pub ground: Ground, } + +#[derive(Component, Debug, Default, Deref, DerefMut)] +pub struct ChunkCenter(PlanetVec2); + +#[derive(Component, Copy, Clone, Default, Debug, Hash, PartialEq, Eq)] +pub struct ChunkIndex { + pub x: i32, + pub y: i32, +} + +impl ChunkIndex { + pub fn new(x: i32, y: i32) -> Self { + Self { x, y } + } +} + +impl From for ChunkCenter { + fn from(value: ChunkIndex) -> Self { + Self(PlanetVec2::new( + value.x as f64 * CHUNK_SIZE as f64, + value.y as f64 * CHUNK_SIZE as f64, + )) + } +} + +impl From for ChunkIndex { + fn from(value: PlanetVec2) -> Self { + Self::new( + ((value.x / CHUNK_SIZE as f64) + 0.5) as i32, + ((value.y / CHUNK_SIZE as f64) + 0.5) as i32, + ) + } +} + +#[derive(Resource, Default, Debug, Deref, DerefMut)] +pub struct LoadedChunks(HashMap); diff --git a/rs/src/planet/terrain.rs b/rs/src/planet/terrain.rs index ea5770e..ace17ec 100644 --- a/rs/src/planet/terrain.rs +++ b/rs/src/planet/terrain.rs @@ -1,9 +1,10 @@ use crate::{ - nav::FnsThatShouldBePub, + nav::heightmap::FnsThatShouldBePub, offloading::{wasm_yield, Offload, OffloadedTask, TaskHandle, TaskOffloader}, planet::{ + chunks::ChunkCenter, terrain::noise::{ChooseAndSmooth, Source, SyncWorley}, - PlanetVec2, PlanetVec3, + PlanetVec2, }, util::{Factory, Spawnable}, }; @@ -235,7 +236,7 @@ impl Spawnable for TerrainObject { transform: Transform, ) -> EntityCommands<'w, 's, 'a> { cmds.spawn(TerrainObject { - mat_mesh_bundle: PbrBundle { + pbr: PbrBundle { mesh: params.mesh.clone(), material: params.material.clone(), transform, @@ -249,7 +250,7 @@ impl Spawnable for TerrainObject { #[derive(Bundle)] pub struct TerrainObject { - pub mat_mesh_bundle: PbrBundle, + pub pbr: PbrBundle, pub rigid_body: RigidBody, pub collider: Collider, pub restitution: Restitution, @@ -260,12 +261,12 @@ pub struct TerrainObject { impl Default for TerrainObject { fn default() -> Self { Self { - mat_mesh_bundle: PbrBundle::default(), + pbr: PbrBundle::default(), rigid_body: RigidBody::Fixed, collider: Collider::default(), restitution: Restitution::new(0.5), friction: Friction::new(0.01), - ccd: Ccd::disabled(), + ccd: Ccd::enabled(), } } } @@ -329,19 +330,21 @@ pub fn spawn_loaded_chunks( let mesh = meshes.add(mesh); let heights = Arc::new(heights); cmds.spawn(( - PlanetVec3::default(), - RigidBody::Fixed, - Collider::from(SharedShape(heights.clone())), - PbrBundle { - mesh, - transform: Transform { - translation: Vec3::new(0.0, 0.0, -768.0), - rotation: Quat::from_rotation_x(FRAC_PI_2), + TerrainObject { + pbr: PbrBundle { + mesh, + transform: Transform { + translation: Vec3::new(0.0, 0.0, -768.0), + rotation: Quat::from_rotation_x(FRAC_PI_2), + ..default() + }, + material: mat.0.clone(), ..default() }, - material: mat.0.clone(), + collider: Collider::from(SharedShape(heights.clone())), ..default() }, + ChunkCenter::default(), Ground { heights }, )); // remove from vec diff --git a/rs/src/util.rs b/rs/src/util.rs index ac588f3..0ffa8d0 100644 --- a/rs/src/util.rs +++ b/rs/src/util.rs @@ -314,8 +314,7 @@ where { } - -#[derive(Clone, Debug)] +#[derive(Copy, Clone, Debug, PartialOrd, PartialEq, Ord, Eq, Hash)] pub enum Todo {} pub struct RonReflectAssetLoader {