From 0b5c8eb8b6b06242ad5c999bb3afdf56e614fbb4 Mon Sep 17 00:00:00 2001
From: rewin <rewin1996@gmail.com>
Date: Thu, 7 Dec 2023 00:55:29 +0300
Subject: [PATCH] Imsprove velocity model and add random walk

---
 src/lib.rs     |  10 +--
 src/physics.rs |  25 +++++-
 src/player.rs  |   4 +-
 src/sheep.rs   | 236 ++++++++++++++++++++++++++++++++++++++++++-------
 4 files changed, 234 insertions(+), 41 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs
index 8e50237..22ddf82 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -51,20 +51,14 @@ impl Plugin for GamePlugin {
             torch::TorchPlugin,
             safe_area::SafeAreaPlugin,
             sprite_material::SpriteMaterialPlugin,
+            sheep::SheepPlugin,
         ));
 
         //For long term updates
         app.insert_resource(Time::<Fixed>::from_seconds(1.0));
 
         app.add_systems(Startup, (test_level::setup, sheep::setup));
-        // TODO: Move to plugin
-        app.add_systems(
-            Update,
-            (
-                sheep::scared_sheeps.before(sheep::sheep_state),
-                sheep::update_scared_sheeps,
-            ),
-        );
+        
     }
 }
 
diff --git a/src/physics.rs b/src/physics.rs
index 85c52e5..159638d 100644
--- a/src/physics.rs
+++ b/src/physics.rs
@@ -4,7 +4,10 @@ pub struct PhysicsPlugin;
 
 impl Plugin for PhysicsPlugin {
     fn build(&self, app: &mut App) {
-        app.add_systems(Update, apply_velocity);
+        app.add_systems(Update, (
+            walk_system,
+            apply_velocity,
+        ).chain());
     }
 }
 
@@ -16,3 +19,23 @@ fn apply_velocity(mut query: Query<(&Velocity, &mut Transform)>, time: Res<Time>
         transform.translation += velocity.0 * time.delta_seconds();
     }
 }
+
+#[derive(Component)]
+pub struct WalkController {
+    pub target_velocity: Vec3,
+    pub acceleration: f32,
+    pub max_speed: f32,
+}
+
+fn walk_system(
+    time: Res<Time>,
+    mut query: Query<(&mut Velocity, &mut WalkController)>,
+) {
+    for (mut velocity, mut controller) in query.iter_mut() {
+        let dspeed = controller.target_velocity - velocity.0;
+        let accel = controller.acceleration.min(dspeed.length() * 100.0);
+    
+        velocity.0 += dspeed.normalize_or_zero() * accel * time.delta_seconds();
+        velocity.0 = velocity.0.clamp_length_max(controller.max_speed);
+    }
+}
\ No newline at end of file
diff --git a/src/player.rs b/src/player.rs
index 3e8d0f0..5fa9d3d 100644
--- a/src/player.rs
+++ b/src/player.rs
@@ -8,8 +8,8 @@ use crate::{
 
 const DOG_PATH: &str = "test/dog.png";
 
-const DOG_SPEED: f32 = 45.0 * 1000.0 / 3600.0; // Sheepdog accepts 35 km/h in reality (but fastest dog can do 67 km/h o.0)
-const DOG_ACCELERATION: f32 = DOG_SPEED * 4.0;
+pub const DOG_SPEED: f32 = 45.0 * 1000.0 / 3600.0; // Sheepdog accepts 35 km/h in reality (but fastest dog can do 67 km/h o.0)
+pub const DOG_ACCELERATION: f32 = DOG_SPEED * 4.0;
 
 pub struct PlayerPlugin;
 
diff --git a/src/sheep.rs b/src/sheep.rs
index dcda7d5..226e2b9 100644
--- a/src/sheep.rs
+++ b/src/sheep.rs
@@ -10,91 +10,244 @@ use rand::Rng;
 
 use crate::{
     get_sprite_rotation,
-    physics::Velocity,
-    player::Bark,
+    physics::{Velocity, WalkController},
+    player::{Bark, DOG_SPEED, DOG_ACCELERATION},
     sprite_material::{create_plane_mesh, SpriteExtension},
     test_level::TEST_LEVEL_SIZE,
 };
 
 const SHEEP_PATH: &str = "test/sheep.png";
 
+const SHEEP_SPEED : f32 = DOG_SPEED * 0.3;
+const SHEEP_ACCELERATION : f32 = SHEEP_SPEED * 3.0;
+
+const RANDOM_WALK_RANGE : f32 = 5.0;
+const RANDOM_WALK_ACCEPT_RADIUS : f32 = 0.5;
+
+const IDLE_FEEDING_TIME : f32 = 1.0;
+const IDLE_FEEDING_TIME_RANGE : f32 = 0.5;
+
+pub struct SheepPlugin;
+
+impl Plugin for SheepPlugin {
+    fn build(&self, app: &mut App) {
+
+        app.init_resource::<StateChance>();
+
+        app.add_systems(
+            Update,
+            (
+                scared_sheeps,
+                update_scared_sheeps,
+            ),
+        );
+
+        app.add_systems(Update, (
+            sheep_state,
+        ));
+
+        //random walk
+        app.add_event::<InitRandomWalk>()
+            .add_systems(Update, (
+                init_random_walk,
+                random_walk_system,
+            ));
+
+        //idle feeding
+        app.add_systems(Update, idle_feeding_system);
+    }
+}
+
 #[derive(Default, PartialEq, Eq, Debug, Clone, Component, Reflect)]
 pub struct Sheep;
 
 #[derive(Default, PartialEq, Debug, Clone, Component, Reflect)]
-pub struct IsScared(bool, f32);
+pub struct IsScared{
+    time : f32
+}
 
-#[derive(Default, PartialEq, Eq, Debug, Clone, Component, Reflect)]
+#[derive(Default, PartialEq, Eq, Debug, Clone, Component, Reflect, Copy)]
 #[reflect(Component, Default)]
 pub enum Decision {
     #[default]
     Idle,
     Feed,
+    IdleFeeding,
+    RandomWalk,
     MoveToSafeZone,
     MoveOutSafeZone,
+    Scared //Mark that ship will not be able to another decision
 }
 
 #[derive(PartialEq, Debug, Clone, Resource, Reflect)]
 #[reflect(Resource, Default)]
 pub struct StateChance {
-    next_state: Vec<(f32, Decision)>,
+    pub next_state: Vec<(f32, Decision)>,
 }
 
 impl Default for StateChance {
     fn default() -> Self {
-        Self {
+        let mut res = Self {
+            //set weights
             next_state: vec![
-                (0.25, Decision::Idle),
-                (0.5, Decision::Feed),
-                (0.75, Decision::MoveToSafeZone),
-                (1.0, Decision::MoveOutSafeZone),
+                (0.0, Decision::Idle),
+                (0.0, Decision::Feed), //zero values for unimplemented things
+                (1.0, Decision::IdleFeeding),
+                (1.0, Decision::RandomWalk),
+                (0.0, Decision::MoveToSafeZone),
+                (0.0, Decision::MoveOutSafeZone),
             ],
+        };
+        res.normalize();
+        res
+    }
+}
+
+impl StateChance {
+    fn normalize(&mut self) {
+        let mut sum = 0.0;
+        for (w, _) in &self.next_state {
+            sum += *w;
+        }
+
+        for (w, _) in &mut self.next_state {
+            *w /= sum;
         }
     }
 }
 
 pub fn scared_sheeps(
+    mut commands: Commands,
     mut event_reader: EventReader<Bark>,
-    mut sheeps: Query<(&Sheep, &Transform, &mut Velocity, &mut IsScared)>,
+    mut sheeps: Query<(Entity, &Transform, &mut WalkController), With<Sheep>>,
 ) {
     if let Some(bark) = event_reader.read().next() {
         let bark_origin = bark.position;
         for mut sheep in &mut sheeps {
             if sheep.1.translation.distance(bark_origin) <= bark.radius {
-                sheep.3 .0 = true;
-                sheep.2 .0 = sheep.1.translation - bark_origin;
-                sheep.2 .0.y = 0.0; //sheep must not fly and be in fixed height
+                let scare = IsScared::default();
+                sheep.2 .target_velocity = (sheep.1.translation - bark_origin).normalize_or_zero() * SHEEP_SPEED;
+                sheep.2 .target_velocity.y = 0.0; //sheep must not fly and be in fixed height
+                commands.entity(sheep.0).insert(scare);
             }
         }
     }
     event_reader.clear();
 }
 
+#[derive(Event)]
+pub struct InitRandomWalk {
+    pub e : Entity,
+}
+
+#[derive(Component)]
+pub struct RandomWalk {
+    pub target : Vec3,
+}
+
+fn init_random_walk(
+    mut commands: Commands,
+    mut event_reader: EventReader<InitRandomWalk>,
+    poses : Query<&Transform, With<Sheep>>
+) {
+
+    let mut rand = rand::thread_rng();
+    for ev in event_reader.read() {
+        if let Ok(t) = poses.get_component::<Transform>(ev.e) {
+            info!("init random walk for {:?}", ev.e);
+            let r = rand.gen_range(0.0..RANDOM_WALK_RANGE);
+            let angle = rand.gen_range(0.0..PI*2.0);
+            
+            commands.entity(ev.e).insert(RandomWalk {
+                target: t.translation + Vec3::new(angle.cos() * r, 0.0, angle.sin() * r),
+            });
+        } else {
+            warn!("init random walk for {:?} failed", ev.e);
+        }
+    }
+    event_reader.clear();
+}
+
+fn random_walk_system(
+    mut commands: Commands,
+    mut random_walks: Query<(Entity, &mut Transform, &mut WalkController, &mut Decision, &RandomWalk)>,
+) {
+    for (e, mut t, mut v, mut dec, rw) in &mut random_walks.iter_mut() {
+        if t.translation.distance(rw.target) < RANDOM_WALK_ACCEPT_RADIUS {
+            v.target_velocity = Vec3::ZERO;
+            commands.entity(e).remove::<RandomWalk>();
+            *dec = Decision::Idle;
+        } else {
+            v.target_velocity = (rw.target - t.translation).normalize() * SHEEP_SPEED;
+        }
+    }
+}
+
 pub fn sheep_state(
-    _next_state: Res<StateChance>,
-    mut sheeps: Query<(&Sheep, &mut Velocity, &mut Decision, &IsScared)>,
+    mut commands: Commands,
+    state_matrix: Res<StateChance>,
+    mut sheeps: Query<(Entity, &mut Decision), With<Sheep>>,
+    mut init_random_walk: EventWriter<InitRandomWalk>,
 ) {
-    for mut sheep in &mut sheeps.iter_mut().filter(|query| !query.3 .0) {
-        *sheep.2 = Decision::Feed;
-        *sheep.1 = Velocity(Vec3 {
-            x: 3.,
-            y: 3.,
-            z: 3.,
-        });
+    let mut rand = rand::thread_rng();
+    for (e, mut dec) in &mut sheeps.iter_mut() {
+        if *dec == Decision::Idle {
+            let p = rand.gen_range(0.0..1.0);
+            let mut sum = 0.0;
+            let mut next_dec = Decision::Idle;
+            for (w, d) in &state_matrix.next_state {
+                sum += *w;
+                if p < sum {
+                    next_dec = *d;
+                    break;
+                }
+            }
+
+            *dec = next_dec;
+
+            match next_dec {
+                Decision::Idle => {
+                    info!("new idle for {:?}", e);
+                },
+                Decision::Feed => {
+                    info!("new feed for {:?}", e);
+                },
+                Decision::RandomWalk => {
+                    info!("new random walk for {:?}", e);
+                    init_random_walk.send(InitRandomWalk { e });
+                },
+                Decision::MoveToSafeZone => {
+                    info!("new move to safe zone for {:?}", e);
+                },
+                Decision::MoveOutSafeZone => {
+                    info!("new move out safe zone for {:?}", e);
+                },
+                Decision::IdleFeeding => {
+                    info!("new idle feeding for {:?}", e);
+                    commands.entity(e).insert(IdleFeeding {
+                        time: rand.gen_range(0.0..IDLE_FEEDING_TIME_RANGE) + IDLE_FEEDING_TIME,
+                    });
+                },
+                Decision::Scared => {
+
+                },
+            }
+        }
     }
 }
 
 pub fn update_scared_sheeps(
+    mut commands: Commands,
     time: Res<Time>,
-    mut sheeps: Query<(&Sheep, &mut Velocity, &mut Decision, &mut IsScared)>,
+    mut sheeps: Query<(Entity, &mut WalkController, &mut Decision, &mut IsScared), With<Sheep>>,
 ) {
-    for mut sheep in sheeps.iter_mut().filter(|q| q.3 .0) {
-        if sheep.3 .1 > 2. {
+    for mut sheep in sheeps.iter_mut() {
+        if sheep.3.time > 2. {
             *sheep.2 = Decision::Idle;
-            *sheep.1 = Velocity::default();
-            *sheep.3 = IsScared::default();
+            sheep.1.target_velocity = Vec3::ZERO;
+            commands.entity(sheep.0).remove::<IsScared>();
         } else {
-            sheep.3 .1 += time.delta_seconds();
+            sheep.3.time += time.delta_seconds();
         }
     }
 }
@@ -142,7 +295,30 @@ pub fn setup(
             Sheep,
             Decision::Idle,
             Velocity::default(),
-            IsScared(false, 0.),
+            WalkController {
+                target_velocity: Vec3::new(0.0, 0.0, 0.0),
+                acceleration: SHEEP_ACCELERATION,
+                max_speed: SHEEP_SPEED,
+            }
         ));
     }
 }
+
+#[derive(Component)]
+pub struct IdleFeeding {
+    pub time: f32,
+}
+
+fn idle_feeding_system(
+    mut commands: Commands,
+    mut sheeps: Query<(Entity, &mut Decision, &mut IdleFeeding)>,
+    time: Res<Time>,
+) {
+    for (e, mut dec, mut idle) in sheeps.iter_mut() {
+        idle.time -= time.delta_seconds();
+        if idle.time < 0.0 {
+            *dec = Decision::Idle;
+            commands.entity(e).remove::<IdleFeeding>();
+        }
+    }
+}
\ No newline at end of file