FyroxEngine/Fyrox
1
0
Fork 0
mirror of https://github.com/FyroxEngine/Fyrox.git synced 2026-10-12 02:02:20 +00:00
Code Issues Packages Projects Releases Wiki Activity

ability to share navmeshes across multiple threads

Browse Source 
- optimized internals of navmesh
- removed lazy graph calculation (replaced it with modification context which recalculates the graph on drop)
This commit is contained in:
Dmitry Stepanov
2023-12-25 11:44:17 +03:00
parent c11638ec79
commit 902aa9a572
6 changed files with 285 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files
editor/src/interaction/navmesh/mod.rs
+12 -11
View File
@@ -412,7 +412,7 @@ impl InteractionMode for EditNavmeshMode {
let graph = &mut engine.scenes[game_scene.scene].graph;
if let Some(selection) = fetch_selection(editor_selection) {
if let Some(navmesh) = graph
if let Some(mut navmesh) = graph
.try_get_mut_of_type::<NavigationalMesh>(selection.navmesh_node())
.map(|n| n.navmesh_mut())
{
@@ -446,8 +446,9 @@ impl InteractionMode for EditNavmeshMode {
if let Some(drag_context) = self.drag_context.as_mut() {
match drag_context {
DragContext::MoveSelection { .. } => {
let mut ctx = navmesh.modify();
for &vertex in &*selection.unique_vertices() {
navmesh.vertices_mut()[vertex] += offset;
ctx.vertices_mut()[vertex] += offset;
}
}
DragContext::EdgeDuplication { vertices, .. } => {
@@ -482,14 +483,14 @@ impl InteractionMode for EditNavmeshMode {
);
if let Some(selection) = fetch_selection(editor_selection) {
let mut gizmo_visible = false;
let mut gizmo_position = Default::default();
if let Some(navmesh) = scene
.graph
.try_get_mut_of_type::<NavigationalMesh>(selection.navmesh_node())
.map(|n| n.navmesh_mut())
{
let mut gizmo_visible = false;
let mut gizmo_position = Default::default();
if let Some(DragContext::EdgeDuplication {
vertices,
opposite_edge,
@@ -539,13 +540,13 @@ impl InteractionMode for EditNavmeshMode {
}
};
}
self.move_gizmo.set_visible(&mut scene.graph, gizmo_visible);
self.move_gizmo
.transform(&mut scene.graph)
.set_scale(scale)
.set_position(gizmo_position);
}
self.move_gizmo.set_visible(&mut scene.graph, gizmo_visible);
self.move_gizmo
.transform(&mut scene.graph)
.set_scale(scale)
.set_position(gizmo_position);
}
}
editor/src/scene/commands/navmesh.rs
+37 -28
View File
@@ -3,6 +3,7 @@ use crate::{
interaction::navmesh::selection::{NavmeshEntity, NavmeshSelection},
scene::{commands::GameSceneContext, Selection},
};
use fyrox::core::parking_lot::RwLockWriteGuard;
use fyrox::{
core::{
algebra::Vector3,
@@ -22,7 +23,10 @@ pub struct AddNavmeshEdgeCommand {
new_selection: Selection,
}
fn fetch_navmesh<'a>(ctx: &'a mut GameSceneContext, node: Handle<Node>) -> &'a mut Navmesh {
fn fetch_navmesh<'a>(
ctx: &'a mut GameSceneContext,
node: Handle<Node>,
) -> RwLockWriteGuard<'a, Navmesh> {
ctx.scene.graph[node]
.as_navigational_mesh_mut()
.navmesh_mut()
@@ -59,19 +63,20 @@ impl GameSceneCommandTrait for AddNavmeshEdgeCommand {
}
fn execute(&mut self, context: &mut GameSceneContext) {
let navmesh = fetch_navmesh(context, self.navmesh_node);
let mut navmesh = fetch_navmesh(context, self.navmesh_node);
match std::mem::replace(&mut self.state, AddNavmeshEdgeCommandState::Undefined) {
AddNavmeshEdgeCommandState::NonExecuted { edge }
| AddNavmeshEdgeCommandState::Reverted { edge } => {
let begin = navmesh.add_vertex(edge.0);
let end = navmesh.add_vertex(edge.1);
navmesh.add_triangle(TriangleDefinition([
let mut ctx = navmesh.modify();
let begin = ctx.add_vertex(edge.0);
let end = ctx.add_vertex(edge.1);
ctx.add_triangle(TriangleDefinition([
self.opposite_edge.a,
begin,
self.opposite_edge.b,
]));
navmesh.add_triangle(TriangleDefinition([begin, end, self.opposite_edge.b]));
ctx.add_triangle(TriangleDefinition([begin, end, self.opposite_edge.b]));
self.state = AddNavmeshEdgeCommandState::Executed;
let navmesh_selection = NavmeshSelection::new(
self.navmesh_node,
@@ -83,6 +88,8 @@ impl GameSceneCommandTrait for AddNavmeshEdgeCommand {
_ => unreachable!(),
}
drop(navmesh);
if self.select {
std::mem::swap(context.selection, &mut self.new_selection);
}
@@ -93,14 +100,15 @@ impl GameSceneCommandTrait for AddNavmeshEdgeCommand {
std::mem::swap(context.selection, &mut self.new_selection);
}
let navmesh = fetch_navmesh(context, self.navmesh_node);
let mut navmesh = fetch_navmesh(context, self.navmesh_node);
match std::mem::replace(&mut self.state, AddNavmeshEdgeCommandState::Undefined) {
AddNavmeshEdgeCommandState::Executed => {
navmesh.pop_triangle();
navmesh.pop_triangle();
let va = navmesh.pop_vertex().unwrap();
let vb = navmesh.pop_vertex().unwrap();
let mut ctx = navmesh.modify();
ctx.pop_triangle();
ctx.pop_triangle();
let va = ctx.pop_vertex().unwrap();
let vb = ctx.pop_vertex().unwrap();
self.state = AddNavmeshEdgeCommandState::Reverted { edge: (vb, va) };
}
_ => unreachable!(),
@@ -137,19 +145,20 @@ impl GameSceneCommandTrait for ConnectNavmeshEdgesCommand {
}
fn execute(&mut self, context: &mut GameSceneContext) {
let navmesh = fetch_navmesh(context, self.navmesh_node);
let mut navmesh = fetch_navmesh(context, self.navmesh_node);
let mut ctx = navmesh.modify();
match std::mem::replace(&mut self.state, ConnectNavmeshEdgesCommandState::Undefined) {
ConnectNavmeshEdgesCommandState::NonExecuted { edges } => {
navmesh.add_triangle(TriangleDefinition([edges[0].a, edges[0].b, edges[1].a]));
navmesh.add_triangle(TriangleDefinition([edges[1].a, edges[1].b, edges[0].a]));
ctx.add_triangle(TriangleDefinition([edges[0].a, edges[0].b, edges[1].a]));
ctx.add_triangle(TriangleDefinition([edges[1].a, edges[1].b, edges[0].a]));
self.state = ConnectNavmeshEdgesCommandState::Executed;
}
ConnectNavmeshEdgesCommandState::Reverted { triangles } => {
let [a, b] = triangles;
navmesh.add_triangle(a);
navmesh.add_triangle(b);
ctx.add_triangle(a);
ctx.add_triangle(b);
self.state = ConnectNavmeshEdgesCommandState::Executed;
}
_ => unreachable!(),
@@ -157,15 +166,13 @@ impl GameSceneCommandTrait for ConnectNavmeshEdgesCommand {
}
fn revert(&mut self, context: &mut GameSceneContext) {
let navmesh = fetch_navmesh(context, self.navmesh_node);
let mut navmesh = fetch_navmesh(context, self.navmesh_node);
let mut ctx = navmesh.modify();
match std::mem::replace(&mut self.state, ConnectNavmeshEdgesCommandState::Undefined) {
ConnectNavmeshEdgesCommandState::Executed => {
self.state = ConnectNavmeshEdgesCommandState::Reverted {
triangles: [
navmesh.pop_triangle().unwrap(),
navmesh.pop_triangle().unwrap(),
],
triangles: [ctx.pop_triangle().unwrap(), ctx.pop_triangle().unwrap()],
}
}
_ => unreachable!(),
@@ -210,7 +217,7 @@ impl GameSceneCommandTrait for DeleteNavmeshVertexCommand {
}
fn execute(&mut self, context: &mut GameSceneContext) {
let navmesh = fetch_navmesh(context, self.navmesh_node);
let mut navmesh = fetch_navmesh(context, self.navmesh_node);
match std::mem::replace(&mut self.state, DeleteNavmeshVertexCommandState::Undefined) {
DeleteNavmeshVertexCommandState::NonExecuted { vertex }
@@ -224,7 +231,7 @@ impl GameSceneCommandTrait for DeleteNavmeshVertexCommand {
}
self.state = DeleteNavmeshVertexCommandState::Executed {
vertex: navmesh.remove_vertex(vertex),
vertex: navmesh.modify().remove_vertex(vertex),
triangles,
vertex_index: vertex,
};
@@ -234,7 +241,7 @@ impl GameSceneCommandTrait for DeleteNavmeshVertexCommand {
}
fn revert(&mut self, context: &mut GameSceneContext) {
let navmesh = fetch_navmesh(context, self.navmesh_node);
let mut navmesh = fetch_navmesh(context, self.navmesh_node);
match std::mem::replace(&mut self.state, DeleteNavmeshVertexCommandState::Undefined) {
DeleteNavmeshVertexCommandState::Executed {
@@ -242,10 +249,12 @@ impl GameSceneCommandTrait for DeleteNavmeshVertexCommand {
vertex_index,
triangles,
} => {
navmesh.insert_vertex(vertex_index as u32, vertex);
let mut ctx = navmesh.modify();
ctx.insert_vertex(vertex_index as u32, vertex);
for triangle in triangles {
navmesh.add_triangle(triangle);
ctx.add_triangle(triangle);
}
self.state = DeleteNavmeshVertexCommandState::Reverted {
@@ -286,8 +295,8 @@ impl MoveNavmeshVertexCommand {
position
}
fn set_position(&self, navmesh: &mut Navmesh, position: Vector3<f32>) {
navmesh.vertices_mut()[self.vertex] = position;
fn set_position(&self, mut navmesh: RwLockWriteGuard<Navmesh>, position: Vector3<f32>) {
navmesh.modify().vertices_mut()[self.vertex] = position;
}
}
examples/navmesh.rs
+3 -2
View File
@@ -168,10 +168,11 @@ impl Plugin for Game {
let navmesh_handle = scene.graph.find_by_name_from_root("Navmesh").unwrap().0;
let navmesh_node = scene.graph[navmesh_handle].as_navigational_mesh_mut();
navmesh_node.debug_draw(&mut scene.drawing_context);
let navmesh = navmesh_node.navmesh_mut();
let navmesh = navmesh_node.navmesh_ref();
self.navmesh_agent.set_target(self.target_position);
let _ = self.navmesh_agent.update(context.dt, navmesh);
let _ = self.navmesh_agent.update(context.dt, &navmesh);
drop(navmesh);
scene.graph[self.agent]
.local_transform_mut()
fyrox-core/src/octree.rs
+15 -9
View File
@@ -166,27 +166,31 @@ impl Octree {
}
}
pub fn point_query(&self, point: Vector3<f32>, buffer: &mut Vec<u32>) {
buffer.clear();
self.point_recursive_query(self.root, point, buffer);
pub fn point_query<C>(&self, point: Vector3<f32>, mut callback: C)
where
C: FnMut(&[u32]),
{
self.point_recursive_query(self.root, point, &mut callback);
}
fn point_recursive_query(
fn point_recursive_query<C>(
&self,
node: Handle<OctreeNode>,
point: Vector3<f32>,
buffer: &mut Vec<u32>,
) {
callback: &mut C,
) where
C: FnMut(&[u32]),
{
match self.nodes.borrow(node) {
OctreeNode::Leaf { indices, bounds } => {
if bounds.is_contains_point(point) {
buffer.extend_from_slice(indices)
(callback)(indices)
}
}
OctreeNode::Branch { bounds, leaves } => {
if bounds.is_contains_point(point) {
for leaf in leaves {
self.point_recursive_query(*leaf, point, buffer)
self.point_recursive_query(*leaf, point, callback)
}
}
}
@@ -292,7 +296,9 @@ mod test {
fn octree_point_query() {
let tree = Octree::new(&get_six_triangles(), 5);
let mut buffer = Vec::new();
tree.point_query(Vector3::new(0.0, 0.0, 0.0), &mut buffer);
tree.point_query(Vector3::new(0.0, 0.0, 0.0), |triangles| {
buffer.extend_from_slice(triangles)
});
assert_eq!(buffer, [0, 1, 2, 3, 0, 1, 2, 3]);
}
src/scene/navmesh.rs
+43 -13
View File
@@ -5,6 +5,7 @@ use crate::{
core::{
color::Color,
math::aabb::AxisAlignedBoundingBox,
parking_lot::RwLock,
pool::Handle,
reflect::prelude::*,
uuid::{uuid, Uuid},
@@ -20,7 +21,26 @@ use crate::{
},
utils::navmesh::Navmesh,
};
use std::ops::{Deref, DerefMut};
use fyrox_core::parking_lot::{RwLockReadGuard, RwLockWriteGuard};
use std::{
ops::{Deref, DerefMut},
sync::Arc,
};
#[derive(Clone, Default, Reflect, Debug)]
struct Container(Arc<RwLock<Navmesh>>);
impl PartialEq for Container {
fn eq(&self, other: &Self) -> bool {
*self.0.read() == *other.0.read()
}
}
impl Visit for Container {
fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
self.0.write().visit(name, visitor)
}
}
/// Navigational mesh (navmesh for short) is a surface which can be used for path finding. Unlike [A* Pathfinder](crate::utils::astar),
/// it can build arbitrary paths on a surface of large polygons, making a path from point A to point B linear (standard pathfinder builds
@@ -83,14 +103,14 @@ use std::ops::{Deref, DerefMut};
/// agent: &mut NavmeshAgent,
/// target: Vector3<f32>,
/// dt: f32,
/// navmesh: &mut NavigationalMesh,
/// navmesh: &NavigationalMesh,
/// ) {
/// // Set the target to follow and the speed.
/// agent.set_target(target);
/// agent.set_speed(1.0);
///
/// // Update the agent.
/// agent.update(dt, navmesh.navmesh_mut()).unwrap();
/// agent.update(dt, &navmesh.navmesh_ref()).unwrap();
///
/// // Print its position - you can use this position as target point of your game character.
/// println!("{}", agent.position());
@@ -112,7 +132,7 @@ use std::ops::{Deref, DerefMut};
pub struct NavigationalMesh {
base: Base,
#[reflect(read_only)]
navmesh: InheritableVariable<Navmesh>,
navmesh: InheritableVariable<Container>,
}
impl TypeUuidProvider for NavigationalMesh {
@@ -151,15 +171,17 @@ impl NodeTrait for NavigationalMesh {
}
fn debug_draw(&self, ctx: &mut SceneDrawingContext) {
for vertex in self.navmesh.vertices().iter() {
let navmesh = self.navmesh.0.read();
for vertex in navmesh.vertices().iter() {
ctx.draw_sphere(*vertex, 6, 6, 0.1, Color::GREEN);
}
for triangle in self.navmesh.triangles().iter() {
for triangle in navmesh.triangles().iter() {
for edge in &triangle.edges() {
ctx.add_line(Line {
begin: self.navmesh.vertices()[edge.a as usize],
end: self.navmesh.vertices()[edge.b as usize],
begin: navmesh.vertices()[edge.a as usize],
end: navmesh.vertices()[edge.b as usize],
color: Color::GREEN,
});
}
@@ -169,13 +191,19 @@ impl NodeTrait for NavigationalMesh {
impl NavigationalMesh {
/// Returns a reference to the inner navigational mesh.
pub fn navmesh_ref(&self) -> &Navmesh {
&self.navmesh
pub fn navmesh_ref(&self) -> RwLockReadGuard<Navmesh> {
self.navmesh.0.read()
}
/// Returns a reference to the inner navigational mesh.
pub fn navmesh_mut(&mut self) -> &mut Navmesh {
&mut self.navmesh
pub fn navmesh_mut(&mut self) -> RwLockWriteGuard<Navmesh> {
self.navmesh.0.write()
}
/// Returns a shared reference to the inner navigational mesh. It could be used to perform
/// off-thread path calculations.
pub fn navmesh(&self) -> Arc<RwLock<Navmesh>> {
self.navmesh.0.clone()
}
}
@@ -203,7 +231,9 @@ impl NavigationalMeshBuilder {
fn build_navigational_mesh(self) -> NavigationalMesh {
NavigationalMesh {
base: self.base_builder.build_base(),
navmesh: self.navmesh.into(),
navmesh: InheritableVariable::new_modified(Container(Arc::new(RwLock::new(
self.navmesh,
)))),
}
}
src/utils/navmesh.rs
+175 -151
View File
@@ -25,10 +25,7 @@ use crate::{
},
};
use fxhash::{FxBuildHasher, FxHashMap};
use std::{
cell::RefCell,
ops::{Deref, DerefMut},
};
use std::ops::{Deref, DerefMut};
#[derive(Clone, Debug, Default, Visit)]
struct Vertex {
@@ -65,8 +62,7 @@ pub struct Navmesh {
octree: Octree,
triangles: Vec<TriangleDefinition>,
vertices: Vec<Vector3<f32>>,
graph: RefCell<Option<Graph<Vertex>>>,
query_buffer: RefCell<Vec<u32>>,
graph: Graph<Vertex>,
}
impl PartialEq for Navmesh {
@@ -116,6 +112,9 @@ impl Visit for Navmesh {
self.octree = Octree::new(&raw_triangles, 32);
}
let graph = make_graph(&self.triangles, &self.vertices);
self.graph = graph;
Ok(())
}
}
@@ -125,6 +124,7 @@ struct Portal {
left: usize,
right: usize,
}
fn triangle_area_2d(a: Vector3<f32>, b: Vector3<f32>, c: Vector3<f32>) -> f32 {
let abx = b[0] - a[0];
let abz = b[2] - a[2];
@@ -204,6 +204,127 @@ fn make_graph(triangles: &[TriangleDefinition], vertices: &[Vector3<f32>]) -> Gr
graph
}
/// A temporary modification context which allows you to modify a navmesh. When the modification
/// context is dropped, it recalculates navigation graph automatically.
pub struct NavmeshModificationContext<'a> {
navmesh: &'a mut Navmesh,
}
impl<'a> Drop for NavmeshModificationContext<'a> {
fn drop(&mut self) {
let graph = make_graph(&self.navmesh.triangles, &self.navmesh.vertices);
self.navmesh.graph = graph;
}
}
impl<'a> NavmeshModificationContext<'a> {
/// Adds the triangle to the navigational mesh and returns its index in the internal array. Vertex indices in
/// the triangle must be valid!
pub fn add_triangle(&mut self, triangle: TriangleDefinition) -> u32 {
let index = self.navmesh.triangles.len();
self.navmesh.triangles.push(triangle);
index as u32
}
/// Removes a triangle at the given index from the navigational mesh.
pub fn remove_triangle(&mut self, index: usize) -> TriangleDefinition {
self.navmesh.triangles.remove(index)
}
/// Removes last triangle from the navigational mesh. Automatically fixes vertex links in the internal
/// navigational graph.
pub fn pop_triangle(&mut self) -> Option<TriangleDefinition> {
if self.navmesh.triangles.is_empty() {
None
} else {
Some(self.remove_triangle(self.navmesh.triangles.len() - 1))
}
}
/// Removes a vertex at the given index from the navigational mesh. All triangles that share the vertex will
/// be also removed.
pub fn remove_vertex(&mut self, index: usize) -> Vector3<f32> {
// Remove triangles that sharing the vertex first.
let mut i = 0;
while i < self.navmesh.triangles.len() {
if self.navmesh.triangles[i]
.indices()
.contains(&(index as u32))
{
self.remove_triangle(i);
} else {
i += 1;
}
}
// Shift vertex indices in triangles. Example:
//
// 0:A 1:B 2:C 3:D 4:E
// [A,B,C], [A,C,D], [A,D,E], [D,C,E]
// [0,1,2], [0,2,3], [0,3,4], [3,2,4]
//
// Remove B.
//
// 0:A 1:C 2:D 3:E
// [A,C,D], [A,D,E], [D,C,E]
// [0,1,2], [0,2,3], [2,1,3]
for triangle in self.navmesh.triangles.iter_mut() {
for other_vertex_index in triangle.indices_mut() {
if *other_vertex_index > index as u32 {
*other_vertex_index -= 1;
}
}
}
self.navmesh.vertices.remove(index)
}
/// Returns a mutable reference to the internal array of vertices.
pub fn vertices_mut(&mut self) -> &mut [Vector3<f32>] {
&mut self.navmesh.vertices
}
/// Adds the vertex to the navigational mesh. The vertex will **not** be connected with any other vertex.
pub fn add_vertex(&mut self, vertex: Vector3<f32>) -> u32 {
let index = self.navmesh.vertices.len();
self.navmesh.vertices.push(vertex);
index as u32
}
/// Removes last vertex from the navigational mesh. All triangles that share the vertex will be also removed.
pub fn pop_vertex(&mut self) -> Option<Vector3<f32>> {
if self.navmesh.vertices.is_empty() {
None
} else {
Some(self.remove_vertex(self.navmesh.vertices.len() - 1))
}
}
/// Inserts the vertex at the given index. Automatically shift indices in triangles to preserve mesh structure.
pub fn insert_vertex(&mut self, index: u32, vertex: Vector3<f32>) {
self.navmesh.vertices.insert(index as usize, vertex);
// Shift vertex indices in triangles. Example:
//
// 0:A 1:C 2:D 3:E
// [A,C,D], [A,D,E], [D,C,E]
// [0,1,2], [0,2,3], [2,1,3]
//
// Insert B.
//
// 0:A 1:B 2:C 3:D 4:E
// [A,C,D], [A,D,E], [D,C,E]
// [0,2,3], [0,3,4], [3,2,4]
for triangle in self.navmesh.triangles.iter_mut() {
for other_vertex_index in triangle.indices_mut() {
if *other_vertex_index >= index {
*other_vertex_index += 1;
}
}
}
}
}
impl Navmesh {
/// Creates new navigation mesh from given set of triangles and vertices. This is
/// low level method that allows to specify triangles and vertices directly. In
@@ -222,11 +343,10 @@ impl Navmesh {
.collect::<Vec<[Vector3<f32>; 3]>>();
Self {
graph: Default::default(),
graph: make_graph(&triangles, &vertices),
triangles,
vertices,
octree: Octree::new(&raw_triangles, 32),
query_buffer: Default::default(),
}
}
@@ -314,38 +434,54 @@ impl Navmesh {
/// navmesh bounds) and `O(n)` complexity in the worst case. `n` here is the number of triangles
/// in the navmesh.
pub fn query_closest(&self, query_point: Vector3<f32>) -> Option<(Vector3<f32>, usize)> {
let mut query_buffer = self.query_buffer.borrow_mut();
self.octree.point_query(query_point, &mut query_buffer);
if query_buffer.is_empty() {
// O(n)
self.query_closest_internal(0..self.triangles.len(), query_point)
} else {
let mut closest = None;
let mut closest_distance = f32::MAX;
self.octree.point_query(query_point, |triangles| {
// O(log(n))
self.query_closest_internal(query_buffer.iter().map(|i| *i as usize), query_point)
self.query_closest_internal(
&mut closest,
&mut closest_distance,
triangles.iter().map(|i| *i as usize),
query_point,
)
});
if closest.is_none() {
// O(n)
self.query_closest_internal(
&mut closest,
&mut closest_distance,
0..self.triangles.len(),
query_point,
)
}
closest
}
fn query_closest_internal(
&self,
closest: &mut Option<(Vector3<f32>, usize)>,
closest_distance: &mut f32,
triangles: impl Iterator<Item = usize>,
query_point: Vector3<f32>,
) -> Option<(Vector3<f32>, usize)> {
let mut closest = None;
let mut closest_distance = f32::MAX;
) {
for triangle_index in triangles {
let triangle = &self.triangles[triangle_index];
let a = self.vertices[triangle[0] as usize];
let b = self.vertices[triangle[1] as usize];
let c = self.vertices[triangle[2] as usize];
let plane = Plane::from_triangle(&a, &b, &c)?;
let Some(plane) = Plane::from_triangle(&a, &b, &c) else {
continue;
};
let projection = plane.project(&query_point);
if math::is_point_inside_triangle(&projection, &[a, b, c]) {
let sqr_distance = query_point.sqr_distance(&projection);
if sqr_distance < closest_distance {
closest_distance = sqr_distance;
closest = Some((projection, triangle_index));
if sqr_distance < *closest_distance {
*closest_distance = sqr_distance;
*closest = Some((projection, triangle_index));
}
}
@@ -359,9 +495,9 @@ impl Navmesh {
if (0.0..=1.0).contains(&t) {
let edge_pt_projection = ray.get_point(t);
let sqr_distance = query_point.sqr_distance(&edge_pt_projection);
if sqr_distance < closest_distance {
closest_distance = sqr_distance;
closest = Some((ray.get_point(t), triangle_index));
if sqr_distance < *closest_distance {
*closest_distance = sqr_distance;
*closest = Some((ray.get_point(t), triangle_index));
}
}
}
@@ -369,14 +505,18 @@ impl Navmesh {
// Also check vertices.
for pt in [a, b, c] {
let sqr_distance = query_point.sqr_distance(&pt);
if sqr_distance < closest_distance {
closest_distance = sqr_distance;
closest = Some((pt, triangle_index));
if sqr_distance < *closest_distance {
*closest_distance = sqr_distance;
*closest = Some((pt, triangle_index));
}
}
}
}
closest
/// Creates a temporary modification context which allows you to modify the navmesh. When the
/// modification context is dropped, it recalculates navigation graph automatically.
pub fn modify(&mut self) -> NavmeshModificationContext {
NavmeshModificationContext { navmesh: self }
}
/// Returns reference to array of triangles.
@@ -384,121 +524,11 @@ impl Navmesh {
&self.triangles
}
/// Adds the triangle to the navigational mesh and returns its index in the internal array. Vertex indices in
/// the triangle must be valid!
pub fn add_triangle(&mut self, triangle: TriangleDefinition) -> u32 {
let index = self.triangles.len();
self.graph = Default::default();
self.triangles.push(triangle);
index as u32
}
/// Removes a triangle at the given index from the navigational mesh.
pub fn remove_triangle(&mut self, index: usize) -> TriangleDefinition {
let triangle = self.triangles.remove(index);
self.graph = Default::default();
triangle
}
/// Removes last triangle from the navigational mesh. Automatically fixes vertex links in the internal
/// navigational graph.
pub fn pop_triangle(&mut self) -> Option<TriangleDefinition> {
if self.triangles.is_empty() {
None
} else {
Some(self.remove_triangle(self.triangles.len() - 1))
}
}
/// Removes a vertex at the given index from the navigational mesh. All triangles that share the vertex will
/// be also removed.
pub fn remove_vertex(&mut self, index: usize) -> Vector3<f32> {
// Remove triangles that sharing the vertex first.
let mut i = 0;
while i < self.triangles.len() {
if self.triangles[i].indices().contains(&(index as u32)) {
self.remove_triangle(i);
} else {
i += 1;
}
}
// Shift vertex indices in triangles. Example:
//
// 0:A 1:B 2:C 3:D 4:E
// [A,B,C], [A,C,D], [A,D,E], [D,C,E]
// [0,1,2], [0,2,3], [0,3,4], [3,2,4]
//
// Remove B.
//
// 0:A 1:C 2:D 3:E
// [A,C,D], [A,D,E], [D,C,E]
// [0,1,2], [0,2,3], [2,1,3]
for triangle in self.triangles.iter_mut() {
for other_vertex_index in triangle.indices_mut() {
if *other_vertex_index > index as u32 {
*other_vertex_index -= 1;
}
}
}
self.vertices.remove(index)
}
/// Returns reference to the internal array of vertices.
pub fn vertices(&self) -> &[Vector3<f32>] {
&self.vertices
}
/// Returns a mutable reference to the internal array of vertices.
pub fn vertices_mut(&mut self) -> &mut [Vector3<f32>] {
self.graph = Default::default();
&mut self.vertices
}
/// Adds the vertex to the navigational mesh. The vertex will **not** be connected with any other vertex.
pub fn add_vertex(&mut self, vertex: Vector3<f32>) -> u32 {
let index = self.vertices.len();
self.vertices.push(vertex);
self.graph = Default::default();
index as u32
}
/// Removes last vertex from the navigational mesh. All triangles that share the vertex will be also removed.
pub fn pop_vertex(&mut self) -> Option<Vector3<f32>> {
if self.vertices.is_empty() {
None
} else {
Some(self.remove_vertex(self.vertices.len() - 1))
}
}
/// Inserts the vertex at the given index. Automatically shift indices in triangles to preserve mesh structure.
pub fn insert_vertex(&mut self, index: u32, vertex: Vector3<f32>) {
self.vertices.insert(index as usize, vertex);
// Shift vertex indices in triangles. Example:
//
// 0:A 1:C 2:D 3:E
// [A,C,D], [A,D,E], [D,C,E]
// [0,1,2], [0,2,3], [2,1,3]
//
// Insert B.
//
// 0:A 1:B 2:C 3:D 4:E
// [A,C,D], [A,D,E], [D,C,E]
// [0,2,3], [0,3,4], [3,2,4]
for triangle in self.triangles.iter_mut() {
for other_vertex_index in triangle.indices_mut() {
if *other_vertex_index >= index {
*other_vertex_index += 1;
}
}
}
self.graph = Default::default();
}
/// Returns shared reference to inner octree.
pub fn octree(&self) -> &Octree {
&self.octree
@@ -528,9 +558,7 @@ impl Navmesh {
to: usize,
path: &mut Vec<Vector3<f32>>,
) -> Result<PathKind, PathError> {
let mut graph = self.graph.borrow_mut();
let graph = graph.get_or_insert_with(|| make_graph(&self.triangles, &self.vertices));
graph.build_positional_path(from, to, path)
self.graph.build_positional_path(from, to, path)
}
/// Tries to pick a triangle by given ray. Returns closest result.
@@ -685,7 +713,7 @@ impl NavmeshAgent {
/// directly, because `update` will call it anyway if target position has moved.
pub fn calculate_path(
&mut self,
navmesh: &mut Navmesh,
navmesh: &Navmesh,
src_point: Vector3<f32>,
dest_point: Vector3<f32>,
) -> Result<PathKind, PathError> {
@@ -704,10 +732,7 @@ impl NavmeshAgent {
}
let mut path_triangle_indices = Vec::new();
let mut graph = navmesh.graph.borrow_mut();
let graph =
graph.get_or_insert_with(|| make_graph(&navmesh.triangles, &navmesh.vertices));
let path_kind = graph.build_indexed_path(
let path_kind = navmesh.graph.build_indexed_path(
src_triangle,
dest_triangle,
&mut path_triangle_indices,
@@ -813,7 +838,7 @@ impl NavmeshAgent {
/// Performs single update tick that moves agent to the target along the path (which is automatically
/// recalculated if target's position has changed).
pub fn update(&mut self, dt: f32, navmesh: &mut Navmesh) -> Result<PathKind, PathError> {
pub fn update(&mut self, dt: f32, navmesh: &Navmesh) -> Result<PathKind, PathError> {
if self.path_dirty {
self.calculate_path(navmesh, self.position, self.target)?;
self.path_dirty = false;
@@ -966,8 +991,7 @@ mod test {
agent.set_target(Vector3::new(3.0, 0.0, 1.0));
agent.update(1.0 / 60.0, &mut navmesh).unwrap();
let graph = navmesh.graph.borrow();
let graph = graph.as_ref().unwrap();
let graph = &navmesh.graph;
assert_eq!(graph.vertices.len(), 6);
assert_eq!(graph.vertices[0].neighbours[0], 1);