add agent_radius to aabb obstacles

vleue · Sep 11, 2024 · 9eaca93 · 9eaca93
1 parent efabded
commit 9eaca93
Show file tree

Hide file tree

Showing 9 changed files with 101 additions and 47 deletions.
diff --git a/examples/demo.rs b/examples/demo.rs
@@ -148,12 +148,8 @@ fn life_of_obstacle(
             if cachable.remove(&entity) {
                 commands.entity(entity).remove::<CachableObstacle>();
             }
-        } else {
-            if example_settings.cache_enabled {
-                if cachable.insert(entity) {
-                    commands.entity(entity).insert(CachableObstacle);
-                }
-            }
+        } else if example_settings.cache_enabled && cachable.insert(entity) {
+            commands.entity(entity).insert(CachableObstacle);
         }
     }
 }

diff --git a/examples/helpers/ui.rs b/examples/helpers/ui.rs
@@ -5,6 +5,7 @@ use vleue_navigator::prelude::*;
 pub enum UiSettings {
     Simplify,
     MergeSteps,
+    AgentRadius,
     Cache,
 }
 
@@ -14,6 +15,8 @@ pub enum UiSettingsButtons {
     SimplifyDec,
     MergeStepsInc,
     MergeStepsDec,
+    AgentRadiusInc,
+    AgentRadiusDec,
     ToggleCache,
 }
 
@@ -137,6 +140,36 @@ pub fn setup_settings<const WITH_CACHE: bool>(mut commands: Commands) {
                     button(" - ", UiSettingsButtons::MergeStepsDec, parent);
                     button(" + ", UiSettingsButtons::MergeStepsInc, parent);
                 });
+            parent
+                .spawn(NodeBundle { ..default() })
+                .with_children(|parent| {
+                    parent.spawn((
+                        TextBundle {
+                            text: Text::from_sections(
+                                [("Agent Radius: ", 30.0), ("{}", 30.0)].into_iter().map(
+                                    |(text, font_size): (&str, f32)| {
+                                        TextSection::new(
+                                            text,
+                                            TextStyle {
+                                                font_size,
+                                                ..default()
+                                            },
+                                        )
+                                    },
+                                ),
+                            ),
+                            style: Style {
+                                margin: UiRect::all(Val::Px(12.0)),
+                                ..default()
+                            },
+                            ..default()
+                        }
+                        .with_text_justify(JustifyText::Right),
+                        UiSettings::AgentRadius,
+                    ));
+                    button(" - ", UiSettingsButtons::AgentRadiusDec, parent);
+                    button(" + ", UiSettingsButtons::AgentRadiusInc, parent);
+                });
             if WITH_CACHE {
                 parent
                     .spawn((
@@ -187,6 +220,9 @@ pub fn display_settings(
                 UiSettings::MergeSteps => {
                     text.sections[1].value = format!("{}", settings.merge_steps)
                 }
+                UiSettings::AgentRadius => {
+                    text.sections[1].value = format!("{}", settings.agent_radius)
+                }
                 UiSettings::Cache => (),
             }
         }
@@ -196,6 +232,7 @@ pub fn display_settings(
             match param {
                 UiSettings::Simplify => (),
                 UiSettings::MergeSteps => (),
+                UiSettings::AgentRadius => (),
                 UiSettings::Cache => {
                     *color = if example_settings.cache_enabled {
                         palettes::tailwind::GREEN_400.into()
@@ -233,6 +270,12 @@ pub fn update_settings<const STEP: u32>(
                     UiSettingsButtons::MergeStepsInc => {
                         settings.merge_steps = (settings.merge_steps + 1).min(5);
                     }
+                    UiSettingsButtons::AgentRadiusDec => {
+                        settings.agent_radius = (settings.agent_radius - 0.5).max(0.0);
+                    }
+                    UiSettingsButtons::AgentRadiusInc => {
+                        settings.agent_radius = (settings.agent_radius + 0.5).min(10.0);
+                    }
                     UiSettingsButtons::ToggleCache => {
                         example_settings.cache_enabled = !example_settings.cache_enabled;
                     }

diff --git a/src/obstacles/aabb.rs b/src/obstacles/aabb.rs
@@ -14,6 +14,7 @@ impl ObstacleSource for Aabb {
         obstacle_transform: &GlobalTransform,
         navmesh_transform: &Transform,
         (up, _shift): (Dir3, f32),
+        agent_radius: f32,
     ) -> Vec<Vec2> {
         let transform = obstacle_transform.compute_transform();
         let world_to_mesh = world_to_mesh(navmesh_transform);

diff --git a/src/obstacles/avian2d.rs b/src/obstacles/avian2d.rs
@@ -21,10 +21,14 @@ impl ObstacleSource for Collider {
         obstacle_transform: &GlobalTransform,
         navmesh_transform: &Transform,
         up: (Dir3, f32),
+        agent_radius: f32,
     ) -> Vec<Vec2> {
-        self.shape_scaled()
-            .as_typed_shape()
-            .get_polygon(obstacle_transform, navmesh_transform, up)
+        self.shape_scaled().as_typed_shape().get_polygon(
+            obstacle_transform,
+            navmesh_transform,
+            up,
+            agent_radius,
+        )
     }
 }
 

diff --git a/src/obstacles/avian3d.rs b/src/obstacles/avian3d.rs
@@ -19,10 +19,14 @@ impl ObstacleSource for Collider {
         obstacle_transform: &GlobalTransform,
         navmesh_transform: &Transform,
         up: (Dir3, f32),
+        agent_radius: f32,
     ) -> Vec<Vec2> {
-        self.shape_scaled()
-            .as_typed_shape()
-            .get_polygon(obstacle_transform, navmesh_transform, up)
+        self.shape_scaled().as_typed_shape().get_polygon(
+            obstacle_transform,
+            navmesh_transform,
+            up,
+            agent_radius,
+        )
     }
 }
 
@@ -32,6 +36,7 @@ trait InnerObstacleSource {
         obstacle_transform: &GlobalTransform,
         navmesh_transform: &Transform,
         up: (Dir3, f32),
+        agent_radius: f32,
     ) -> Vec<Vec2>;
 }
 

diff --git a/src/obstacles/cached.rs b/src/obstacles/cached.rs
@@ -37,12 +37,19 @@ impl<T: ObstacleSource> ObstacleSource for CachedObstacle<T> {
         obstacle_transform: &GlobalTransform,
         navmesh_transform: &Transform,
         up: (Dir3, f32),
+        agent_radius: f32,
     ) -> Vec<Vec2> {
         self.polygon
             .get_or_init(|| {
-                T::get_polygon(&self.source, obstacle_transform, navmesh_transform, up)
-                    .into_iter()
-                    .collect::<Vec<_>>()
+                T::get_polygon(
+                    &self.source,
+                    obstacle_transform,
+                    navmesh_transform,
+                    up,
+                    agent_radius,
+                )
+                .into_iter()
+                .collect::<Vec<_>>()
             })
             .clone()
     }

diff --git a/src/obstacles/mod.rs b/src/obstacles/mod.rs
@@ -22,5 +22,6 @@ pub trait ObstacleSource: Component + Clone {
         obstacle_transform: &GlobalTransform,
         navmesh_transform: &Transform,
         up: (Dir3, f32),
+        agent_radius: f32,
     ) -> Vec<Vec2>;
 }
diff --git a/src/obstacles/primitive.rs b/src/obstacles/primitive.rs
@@ -77,6 +77,7 @@ impl ObstacleSource for PrimitiveObstacle {
         obstacle_transform: &GlobalTransform,
         navmesh_transform: &Transform,
         (up, _shift): (Dir3, f32),
+        agent_radius: f32,
     ) -> Vec<Vec2> {
         let transform = obstacle_transform.compute_transform();
         let world_to_mesh = world_to_mesh(navmesh_transform);
@@ -94,36 +95,32 @@ impl ObstacleSource for PrimitiveObstacle {
         let to_navmesh = |v: Vec3| world_to_mesh.transform_point3(v).xy();
 
         match self {
-            PrimitiveObstacle::Rectangle(primitive) => vec![
-                to_navmesh(to_world(vec2(
-                    -primitive.half_size.x,
-                    -primitive.half_size.y,
-                ))),
-                to_navmesh(to_world(vec2(
-                    -primitive.half_size.x,
-                    primitive.half_size.y,
-                ))),
-                to_navmesh(to_world(vec2(primitive.half_size.x, primitive.half_size.y))),
-                to_navmesh(to_world(vec2(
-                    primitive.half_size.x,
-                    -primitive.half_size.y,
-                ))),
-            ],
+            PrimitiveObstacle::Rectangle(primitive) => {
+                let x = primitive.half_size.x + agent_radius;
+                let y = primitive.half_size.y + agent_radius;
+                vec![
+                    to_navmesh(to_world(vec2(-x, -y))),
+                    to_navmesh(to_world(vec2(-x, y))),
+                    to_navmesh(to_world(vec2(x, y))),
+                    to_navmesh(to_world(vec2(x, -y))),
+                ]
+            }
             PrimitiveObstacle::Circle(primitive) => {
-                copypasta::ellipse_inner(vec2(primitive.radius, primitive.radius), RESOLUTION)
+                let with_radius = primitive.radius + agent_radius;
+                copypasta::ellipse_inner(vec2(with_radius, with_radius), RESOLUTION)
                     .map(|v| to_navmesh(to_world(v)))
                     .collect()
             }
             PrimitiveObstacle::Ellipse(primitive) => {
-                copypasta::ellipse_inner(primitive.half_size, RESOLUTION)
+                copypasta::ellipse_inner(primitive.half_size + agent_radius, RESOLUTION)
                     .map(|v| to_navmesh(to_world(v)))
                     .collect()
             }
             PrimitiveObstacle::CircularSector(primitive) => {
                 let mut arc = copypasta::arc_2d_inner(
                     0.0,
                     primitive.arc.angle() as f64,
-                    primitive.arc.radius,
+                    primitive.arc.radius + agent_radius,
                     RESOLUTION,
                 )
                 .map(|v| to_navmesh(to_world(v)))
@@ -134,7 +131,7 @@ impl ObstacleSource for PrimitiveObstacle {
             PrimitiveObstacle::CircularSegment(primitive) => copypasta::arc_2d_inner(
                 0.0,
                 primitive.arc.angle() as f64,
-                primitive.arc.radius,
+                primitive.arc.radius + agent_radius,
                 RESOLUTION,
             )
             .map(|v| to_navmesh(to_world(v)))
@@ -143,7 +140,7 @@ impl ObstacleSource for PrimitiveObstacle {
                 let mut points = copypasta::arc_2d_inner(
                     0.0,
                     std::f64::consts::PI,
-                    primitive.radius,
+                    primitive.radius + agent_radius,
                     RESOLUTION,
                 )
                 .map(|v| to_navmesh(to_world(v + primitive.half_length * Vec2::Y)))
@@ -152,7 +149,7 @@ impl ObstacleSource for PrimitiveObstacle {
                     copypasta::arc_2d_inner(
                         0.0,
                         std::f64::consts::PI,
-                        primitive.radius,
+                        primitive.radius + agent_radius,
                         RESOLUTION,
                     )
                     .map(|v| {
@@ -167,21 +164,18 @@ impl ObstacleSource for PrimitiveObstacle {
             PrimitiveObstacle::RegularPolygon(primitive) => (0..=primitive.sides)
                 .map(|p| {
                     copypasta::single_circle_coordinate(
-                        primitive.circumcircle.radius,
+                        primitive.circumcircle.radius + agent_radius,
                         primitive.sides as u32,
                         p,
                     )
                 })
                 .map(|v| to_navmesh(to_world(v)))
                 .collect(),
             PrimitiveObstacle::Rhombus(primitive) => {
+                let x = primitive.half_diagonals.x + agent_radius;
+                let y = primitive.half_diagonals.y + agent_radius;
                 [(1.0, 0.0), (0.0, 1.0), (-1.0, 0.0), (0.0, -1.0)]
-                    .map(|(sign_x, sign_y)| {
-                        Vec2::new(
-                            primitive.half_diagonals.x * sign_x,
-                            primitive.half_diagonals.y * sign_y,
-                        )
-                    })
+                    .map(|(sign_x, sign_y)| Vec2::new(x * sign_x, y * sign_y))
                     .into_iter()
                     .map(|v| to_navmesh(to_world(v)))
                     .collect()

diff --git a/src/updater.rs b/src/updater.rs
@@ -93,6 +93,8 @@ pub struct NavMeshSettings {
     ///
     /// if feature `detailed-layers` is enabled, it's also used for path finding to change the traversal cost of this layer.
     pub scale: Vec2,
+    /// Agent radius used to increase the size of obstacles so that path can avoid them
+    pub agent_radius: f32,
 }
 
 impl Default for NavMeshSettings {
@@ -108,6 +110,7 @@ impl Default for NavMeshSettings {
             layer: None,
             stitches: vec![],
             scale: Vec2::ONE,
+            agent_radius: 0.0,
         }
     }
 }
@@ -157,9 +160,9 @@ fn build_navmesh<T: ObstacleSource>(
     let up = (mesh_transform.forward(), settings.upward_shift);
     let base = if settings.cached.is_none() {
         let mut base = settings.fixed;
-        let obstacle_polys = cached_obstacles
-            .iter()
-            .map(|(transform, obstacle)| obstacle.get_polygon(transform, &mesh_transform, up));
+        let obstacle_polys = cached_obstacles.iter().map(|(transform, obstacle)| {
+            obstacle.get_polygon(transform, &mesh_transform, up, settings.agent_radius)
+        });
         base.add_obstacles(obstacle_polys);
         if settings.simplify != 0.0 {
             base.simplify(settings.simplify);
@@ -175,7 +178,7 @@ fn build_navmesh<T: ObstacleSource>(
         .iter()
         .map(|(transform, obstacle)| {
             obstacle
-                .get_polygon(transform, &mesh_transform, up)
+                .get_polygon(transform, &mesh_transform, up, settings.agent_radius)
                 .into_iter()
                 .map(|v| v / scale)
                 .collect()