fixed collect_portals_along_path + added temp example

- bugged `straighten_path`

examples/navmesh.rs

@@ -0,0 +1,298 @@
+//! Example 01. Simple scene.
+//!
+//! Difficulty: Easy.
+//!
+//! This example shows how to create simple scene with animated model.
+
+use fyrox::event_loop::EventLoop;
+use fyrox::scene::camera::CameraBuilder;
+use fyrox::scene::node::NodeTrait;
+use fyrox::{
+    asset::manager::ResourceManager,
+    core::{
+        algebra::{Matrix4, Point3, UnitQuaternion, Vector2, Vector3},
+        arrayvec::ArrayVec,
+        color::Color,
+        pool::Handle,
+        sstorage::ImmutableString,
+    },
+    dpi::LogicalPosition,
+    engine::{executor::Executor, GraphicsContext, GraphicsContextParams},
+    event::{ElementState, Event, WindowEvent},
+    gui::{
+        message::MessageDirection,
+        text::{TextBuilder, TextMessage},
+        widget::WidgetBuilder,
+        BuildContext, UiNode,
+    },
+    keyboard::KeyCode,
+    material::{Material, PropertyValue, SharedMaterial},
+    plugin::{Plugin, PluginConstructor, PluginContext},
+    resource::model::{Model, ModelResourceExtension},
+    scene::{
+        base::BaseBuilder,
+        debug::Line,
+        graph::physics::{Intersection, RayCastOptions},
+        mesh::{
+            surface::{SurfaceBuilder, SurfaceData, SurfaceSharedData},
+            MeshBuilder,
+        },
+        node::Node,
+        transform::TransformBuilder,
+        Scene,
+    },
+    utils::navmesh::NavmeshAgent,
+};
+use winit::keyboard::PhysicalKey;
+
+fn create_ui(ctx: &mut BuildContext) -> Handle<UiNode> {
+    TextBuilder::new(WidgetBuilder::new()).build(ctx)
+}
+
+struct GameScene {
+    scene: Scene,
+    agent: Handle<Node>,
+    cursor: Handle<Node>,
+    camera: Handle<Node>,
+}
+
+async fn create_scene(resource_manager: ResourceManager) -> GameScene {
+    let mut scene = Scene::new();
+
+    // Camera is our eyes in the world - you won't see anything without it.
+    let camera = CameraBuilder::new(
+        BaseBuilder::new().with_local_transform(
+            TransformBuilder::new()
+                .with_local_position(Vector3::new(0.0, 5.0, 0.0))
+                .build(),
+        ),
+    )
+    .build(&mut scene.graph);
+
+    scene.graph[camera]
+        .local_transform_mut()
+        .set_rotation(UnitQuaternion::from_axis_angle(
+            &Vector3::x_axis(),
+            90.0f32.to_radians(),
+        ));
+
+    resource_manager
+        .request::<Model, _>("examples/data/navmesh_scene.rgs")
+        .await
+        .unwrap()
+        .instantiate(&mut scene);
+
+    let mut cursor_material = Material::standard();
+    cursor_material
+        .set_property(
+            &ImmutableString::new("diffuseColor"),
+            PropertyValue::Color(Color::opaque(255, 0, 0)),
+        )
+        .unwrap();
+
+    let cursor = MeshBuilder::new(BaseBuilder::new())
+        .with_surfaces(vec![SurfaceBuilder::new(SurfaceSharedData::new(
+            SurfaceData::make_sphere(10, 10, 0.1, &Matrix4::identity()),
+        ))
+        .with_material(SharedMaterial::new(cursor_material))
+        .build()])
+        .build(&mut scene.graph);
+
+    let mut agent_material = Material::standard();
+    agent_material
+        .set_property(
+            &ImmutableString::new("diffuseColor"),
+            PropertyValue::Color(Color::opaque(0, 200, 0)),
+        )
+        .unwrap();
+
+    let agent = MeshBuilder::new(
+        BaseBuilder::new().with_local_transform(
+            TransformBuilder::new()
+                .with_local_scale(Vector3::new(1.0, 2.0, 1.0))
+                .build(),
+        ),
+    )
+    .with_surfaces(vec![SurfaceBuilder::new(SurfaceSharedData::new(
+        SurfaceData::make_sphere(10, 10, 0.2, &Matrix4::identity()),
+    ))
+    .with_material(SharedMaterial::new(agent_material))
+    .build()])
+    .build(&mut scene.graph);
+
+    GameScene {
+        scene,
+        cursor,
+        agent,
+        camera,
+    }
+}
+
+#[derive(Default)]
+struct InputController {
+    rotate_left: bool,
+    rotate_right: bool,
+}
+
+struct Game {
+    input_controller: InputController,
+    scene_handle: Handle<Scene>,
+    agent: Handle<Node>,
+    cursor: Handle<Node>,
+    camera: Handle<Node>,
+    target_position: Vector3<f32>,
+    mouse_position: Vector2<f32>,
+    navmesh_agent: NavmeshAgent,
+    debug_text: Handle<UiNode>,
+}
+
+impl Plugin for Game {
+    fn update(&mut self, context: &mut PluginContext) {
+        // Use stored scene handle to borrow a mutable reference of scene in
+        // engine.
+        let scene = &mut context.scenes[self.scene_handle];
+
+        scene.drawing_context.clear_lines();
+
+        if let GraphicsContext::Initialized(ref graphics_context) = context.graphics_context {
+            let ray = scene.graph[self.camera].as_camera().make_ray(
+                self.mouse_position,
+                graphics_context.renderer.get_frame_bounds(),
+            );
+
+            let mut buffer = ArrayVec::<Intersection, 64>::new();
+            scene.graph.physics.cast_ray(
+                RayCastOptions {
+                    ray_origin: Point3::from(ray.origin),
+                    ray_direction: ray.dir,
+                    max_len: 9999.0,
+                    groups: Default::default(),
+                    sort_results: true,
+                },
+                &mut buffer,
+            );
+
+            if let Some(first) = buffer.first() {
+                self.target_position = first.position.coords;
+                scene.graph[self.cursor]
+                    .local_transform_mut()
+                    .set_position(self.target_position);
+            }
+        }
+
+        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 last = std::time::Instant::now();
+        self.navmesh_agent.set_target(self.target_position);
+        let _ = self.navmesh_agent.update(context.dt, navmesh);
+        let agent_time = std::time::Instant::now() - last;
+
+        scene.graph[self.agent]
+            .local_transform_mut()
+            .set_position(self.navmesh_agent.position());
+
+        for pts in self.navmesh_agent.path().windows(2) {
+            scene.drawing_context.add_line(Line {
+                begin: pts[0],
+                end: pts[1],
+                color: Color::opaque(255, 0, 0),
+            });
+        }
+
+        if let GraphicsContext::Initialized(ref graphics_context) = context.graphics_context {
+            let fps = graphics_context.renderer.get_statistics().frames_per_second;
+            let text = format!(
+                "Example 12 - Navigation Mesh\nFPS: {}\nAgent time: {:?}",
+                fps, agent_time
+            );
+            context.user_interface.send_message(TextMessage::text(
+                self.debug_text,
+                MessageDirection::ToWidget,
+                text,
+            ));
+        }
+    }
+
+    fn on_os_event(&mut self, event: &Event<()>, context: PluginContext) {
+        if let Event::WindowEvent { event, .. } = event {
+            match event {
+                WindowEvent::KeyboardInput { event: input, .. } => match input.physical_key {
+                    PhysicalKey::Code(KeyCode::KeyA) => {
+                        self.input_controller.rotate_left = input.state == ElementState::Pressed
+                    }
+                    PhysicalKey::Code(KeyCode::KeyD) => {
+                        self.input_controller.rotate_right = input.state == ElementState::Pressed
+                    }
+                    _ => (),
+                },
+                WindowEvent::CursorMoved { position, .. } => {
+                    if let GraphicsContext::Initialized(ref graphics_context) =
+                        context.graphics_context
+                    {
+                        let p: LogicalPosition<f32> =
+                            position.to_logical(graphics_context.window.scale_factor());
+                        self.mouse_position = Vector2::new(p.x, p.y);
+                    }
+                }
+                _ => (),
+            }
+        }
+    }
+}
+
+struct GameConstructor;
+
+impl PluginConstructor for GameConstructor {
+    fn create_instance(
+        &self,
+        _override_scene: Option<&str>,
+        context: PluginContext,
+    ) -> Box<dyn Plugin> {
+        let debug_text = create_ui(&mut context.user_interface.build_ctx());
+
+        // Create test scene.
+        let GameScene {
+            scene,
+            agent,
+            cursor,
+            camera,
+        } = fyrox::core::futures::executor::block_on(create_scene(
+            context.resource_manager.clone(),
+        ));
+
+        // Add scene to engine - engine will take ownership over scene and will return
+        // you a handle to scene which can be used later on to borrow it and do some
+        // actions you need.
+        let scene_handle = context.scenes.add(scene);
+
+        let mut navmesh_agent = NavmeshAgent::new();
+        navmesh_agent.set_speed(0.75);
+
+        Box::new(Game {
+            input_controller: Default::default(),
+            scene_handle,
+            agent,
+            cursor,
+            camera,
+            target_position: Default::default(),
+            mouse_position: Default::default(),
+            navmesh_agent,
+            debug_text,
+        })
+    }
+}
+
+fn main() {
+    let mut executor = Executor::from_params(
+        EventLoop::new().unwrap(),
+        GraphicsContextParams {
+            window_attributes: Default::default(),
+            vsync: true,
+        },
+    );
+    executor.add_plugin_constructor(GameConstructor);
+    executor.run()
+}

src/utils/navmesh.rs

@@ -73,6 +73,36 @@ impl Visit for Navmesh {
     }
 }
 
+#[derive(Copy, Clone, Debug)]
+struct Portal {
+    left: usize,
+    right: usize,
+}
+fn signed_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];
+    let acx = c[0] - a[0];
+    let acz = c[2] - a[2];
+    acx * abz - abx * acz
+}
+
+/*
+
+#[derive(PartialEq, Clone, Copy, Eq)]
+enum Winding {
+    Clockwise,
+    CounterClockwise,
+}
+
+
+fn winding(a: Vector3<f32>, b: Vector3<f32>, c: Vector3<f32>) -> Winding {
+    if signed_triangle_area_2d(a, b, c) > 0.0 {
+        Winding::Clockwise
+    } else {
+        Winding::CounterClockwise
+    }
+}
+*/
 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
@@ -417,14 +447,13 @@ impl Navmesh {
 
     // Collects indices of triangles that has path vertices in them.
     // TODO: This method uses brute-force algorithm with bad complexity.
-    fn make_triangle_strip(&self, path_vertices: &[usize]) -> Vec<usize> {
-        let mut triangle_indices = Vec::new();
+    fn make_triangle_strip(&self, path_vertices: &[usize]) -> FxHashSet<usize> {
+        let mut triangle_indices = FxHashSet::default();
 
         for path_vertex in path_vertices.iter() {
-            'triangle_loop: for triangle in self.triangles.iter() {
+            for (triangle_index, triangle) in self.triangles.iter().enumerate() {
                 if triangle.0.contains(&(*path_vertex as u32)) {
-                    triangle_indices.push(*path_vertex);
-                    break 'triangle_loop;
+                    triangle_indices.insert(triangle_index);
                 }
             }
         }
@@ -440,50 +469,51 @@ impl Navmesh {
         src_triangle_index: usize,
         dest_triangle_index: usize,
         dest_position: Vector3<f32>,
-        path_triangles: &[usize],
-    ) -> Vec<(usize, usize)> {
+        path_triangles: &FxHashSet<usize>,
+    ) -> Vec<Portal> {
         let mut portals = Vec::new();
 
+        let mut visited_triangles = FxHashSet::default();
         let vertices = self.vertices();
         let mut current_triangle_index = src_triangle_index;
-        'outer_loop: loop {
+        let mut iter_count = 0;
+        while current_triangle_index != dest_triangle_index && iter_count < path_triangles.len() {
+            visited_triangles.insert(current_triangle_index);
             let current_triangle = &self.triangles[current_triangle_index];
             let a = vertices[current_triangle[0] as usize].position;
             let b = vertices[current_triangle[1] as usize].position;
             let c = vertices[current_triangle[2] as usize].position;
             let ca = c - a;
             let ba = b - a;
-            let triangle_normal = ca.cross(&ba);
-            'edge_loop: for edge in current_triangle.edges() {
-                let a = vertices[edge.a as usize].position;
-                let b = vertices[edge.b as usize].position;
+            let triangle_normal = ba.cross(&ca);
+            for current_triangle_edge in current_triangle.edges() {
+                let a = vertices[current_triangle_edge.a as usize].position;
+                let b = vertices[current_triangle_edge.b as usize].position;
                 let edge_vector = b - a;
                 let edge_normal = edge_vector.cross(&triangle_normal);
                 let target_vector = dest_position - a;
-                let looks_towards_target = edge_normal.dot(&target_vector) >= 0.0;
+                let looks_towards_target = edge_normal.dot(&target_vector) > 0.0;
                 if looks_towards_target {
-                    portals.push((edge.a as usize, edge.b as usize));
+                    portals.push(Portal {
+                        left: current_triangle_edge.a as usize,
+                        right: current_triangle_edge.b as usize,
+                    });
 
                     // Find adjacent triangle to continue portals searching.
-                    'triangle_loop: for triangle_index in path_triangles.iter() {
-                        if *triangle_index == dest_triangle_index {
-                            break 'outer_loop;
-                        }
-
-                        if *triangle_index != current_triangle_index {
-                            let triangle = self.triangles[*triangle_index];
-                            for other_edge in triangle.edges() {
-                                if edge == other_edge {
-                                    current_triangle_index = *triangle_index;
+                    'triangle_loop: for other_triangle_index in path_triangles.iter() {
+                        if !visited_triangles.contains(other_triangle_index) {
+                            let other_triangle = self.triangles[*other_triangle_index];
+                            for other_triangle_edge in other_triangle.edges() {
+                                if current_triangle_edge == other_triangle_edge {
+                                    current_triangle_index = *other_triangle_index;
                                     break 'triangle_loop;
                                 }
                             }
                         }
                     }
-
-                    break 'edge_loop;
                 }
             }
+            iter_count += 1;
         }
 
         portals
@@ -631,6 +661,7 @@ impl NavmeshAgent {
                     navmesh,
                     src_triangle,
                     dest_triangle,
+                    src_point_on_navmesh,
                     dest_point_on_navmesh,
                     &path_vertex_indices,
                 );
@@ -647,10 +678,17 @@ impl NavmeshAgent {
         navmesh: &Navmesh,
         src_triangle_index: usize,
         dest_triangle_index: usize,
+        src_position: Vector3<f32>,
         dest_position: Vector3<f32>,
         path_vertices: &[usize],
     ) {
+        self.path.clear();
+        self.path.push(src_position);
+
+        let vertices = navmesh.vertices();
+
         let path_triangles = navmesh.make_triangle_strip(path_vertices);
+
         let portals = navmesh.collect_portals_along_path(
             src_triangle_index,
             dest_triangle_index,
@@ -658,9 +696,74 @@ impl NavmeshAgent {
             &path_triangles,
         );
 
-        for _portal in &portals[1..] {
-            // TODO
+        let mut portal_apex = src_position;
+        let mut portal_left = src_position;
+        let mut portal_right = src_position;
+
+        let mut apex_index;
+        let mut left_index = 0;
+        let mut right_index = 0;
+
+        let mut i = 0;
+        while i < portals.len() {
+            let left = vertices[portals[i].left].position;
+            let right = vertices[portals[i].right].position;
+
+            // Update right vertex.
+            if signed_triangle_area_2d(portal_apex, portal_right, right) <= 0.0 {
+                if portal_apex == portal_right
+                    || signed_triangle_area_2d(portal_apex, portal_left, right) > 0.0
+                {
+                    // Tighten the funnel.
+                    portal_right = right;
+                    right_index = i;
+                } else {
+                    // Right over left, insert left to path and restart scan from portal left point.
+                    self.path.push(portal_left);
+                    // Make current left the new apex.
+                    portal_apex = portal_left;
+                    apex_index = left_index;
+                    // Reset portal
+                    portal_left = portal_apex;
+                    portal_right = portal_apex;
+                    left_index = apex_index;
+                    right_index = apex_index;
+                    // Restart scan
+                    i = apex_index;
+                    dbg!(i);
+                    continue;
+                }
+            }
+
+            // Update left vertex.
+            if signed_triangle_area_2d(portal_apex, portal_left, left) >= 0.0 {
+                if portal_apex == portal_left
+                    || signed_triangle_area_2d(portal_apex, portal_right, left) < 0.0
+                {
+                    // Tighten the funnel.
+                    portal_left = left;
+                    left_index = i;
+                } else {
+                    // Left over right, insert right to path and restart scan from portal right point.
+                    self.path.push(portal_right);
+                    // Make current right the new apex.
+                    portal_apex = portal_right;
+                    apex_index = right_index;
+                    // Reset portal
+                    portal_left = portal_apex;
+                    portal_right = portal_apex;
+                    left_index = apex_index;
+                    right_index = apex_index;
+                    // Restart scan
+                    i = apex_index;
+                    continue;
+                }
+            }
+
+            i += 1;
         }
+
+        self.path.push(dest_position);
     }
 
     /// Performs single update tick that moves agent to the target along the path (which is automatically
@@ -821,6 +924,49 @@ mod test {
         )
     }
 
+    #[test]
+    fn test_collect_portals() {
+        let mut navmesh = Navmesh::new(
+            &[
+                TriangleDefinition([0, 1, 3]),
+                TriangleDefinition([1, 2, 3]),
+                TriangleDefinition([2, 5, 3]),
+                TriangleDefinition([2, 4, 5]),
+                TriangleDefinition([4, 7, 5]),
+                TriangleDefinition([4, 6, 7]),
+            ],
+            &[
+                Vector3::new(0.0, 0.0, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+                Vector3::new(1.0, 0.0, 1.0),
+                Vector3::new(1.0, 0.0, 0.0),
+                Vector3::new(2.0, 0.0, 1.0),
+                Vector3::new(2.0, 0.0, 0.0),
+                Vector3::new(3.0, 0.0, 1.0),
+                Vector3::new(3.0, 0.0, 0.0),
+            ],
+        );
+
+        let start = 0;
+        let end = 6;
+        let dest_position = navmesh.vertices()[end].position;
+
+        let mut path = Vec::new();
+        let mut indices = Vec::new();
+        navmesh
+            .pathfinder
+            .build_and_convert(start, end, &mut path, |i, v| {
+                indices.push(i);
+                v.position
+            })
+            .unwrap();
+        dbg!(&indices);
+        let strip = navmesh.make_triangle_strip(&indices);
+        dbg!(&strip);
+        let portals = navmesh.collect_portals_along_path(0, 5, dest_position, &strip);
+        dbg!(&portals);
+    }
+
     #[test]
     fn test_remove_triangle() {
         let mut navmesh = make_navmesh();