1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134

use draw2d::graphics::vertex::Vertex2d;

type Vec2 = nalgebra::Vector2<f32>;

pub struct Bounds {
    pub left: f32,
    pub right: f32,
    pub bottom: f32,
    pub top: f32,
    pub margin: f32,
}

#[derive(Debug, Copy, Clone)]
pub struct Vehicle {
    pos: Vec2,
    vel: Vec2,
    accel: Vec2,
}

impl Vehicle {
    const MAX_FORCE: f32 = 400.0;
    const MAX_VEL: f32 = 15.0;

    pub fn new(pos: [f32; 2], vel: [f32; 2]) -> Vehicle {
        Self {
            pos: pos.into(),
            vel: vel.into(),
            accel: Vec2::new(0.0, 0.0),
        }
    }

    /// Step a vehicle forward in time with an euler integrator.
    ///
    /// # Params
    ///
    /// `dt` is the timestep used for integration. Behavior can become unstable
    /// for large `dt` or for large velocities and accelerations.
    pub fn integrate(&mut self, dt: f32) {
        self.vel = (self.vel + self.accel * dt).limit(Self::MAX_VEL);
        self.pos += self.vel * dt;
        self.accel *= 0.0;
    }

    /// Enforce bounds, don't let the vehicles escape the screen.
    pub fn enforce_bounds(&mut self, bounds: &Bounds) {
        const TIME_TO_ENFORCE: f32 = 0.25;

        if self.pos.x < bounds.left + bounds.margin {
            self.seek_vel(
                Vec2::new(Self::MAX_VEL, self.vel.y).limit(Self::MAX_VEL),
                TIME_TO_ENFORCE,
            );
        } else if self.pos.x > bounds.right - bounds.margin {
            self.seek_vel(
                Vec2::new(-Self::MAX_VEL, self.vel.y).limit(Self::MAX_VEL),
                TIME_TO_ENFORCE,
            );
        }

        if self.pos.y < bounds.bottom + bounds.margin {
            self.seek_vel(
                Vec2::new(self.vel.x, Self::MAX_VEL).limit(Self::MAX_VEL),
                TIME_TO_ENFORCE,
            );
        } else if self.pos.y > bounds.top - bounds.margin {
            self.seek_vel(
                Vec2::new(self.vel.x, -Self::MAX_VEL).limit(Self::MAX_VEL),
                TIME_TO_ENFORCE,
            );
        }
    }

    /// Seek a target velocity.
    ///
    /// # Params
    ///
    /// - `target_vel` is the desired velocity, if not otherwise effected the
    ///   vehicle will eventually reach the target velocity.
    /// - `secs_to_target` is how many seconds (floating point) it should take
    ///   for the vehicle to reach the target velocity.
    pub fn seek_vel(&mut self, target_vel: Vec2, secs_to_target: f32) {
        let error: Vec2 = (target_vel - self.vel).scale(1.0 / secs_to_target);
        self.apply_force(error);
    }

    /// Apply a force to a vehicle.
    ///
    /// Forces are reset to zero after each integration.
    pub fn apply_force(&mut self, force: Vec2) {
        self.accel += force.limit(Self::MAX_FORCE);
    }

    /// Draw the vehicle as a triangle to a single graphics layer.
    pub fn draw(&self) -> [Vertex2d; 3] {
        const SIZE: f32 = 1.0;

        let look = self.vel.normalize();
        let look_right = Vec2::new(look.y, -look.x);
        let look_left = -look_right;

        let front = self.pos + look * SIZE * 0.5;
        let right = self.pos + (look_right * SIZE * 0.15) + (look * -0.5);
        let left = self.pos + (look_left * SIZE * 0.15) + (look * -0.5);

        [
            Vertex2d {
                pos: front.into(),
                ..Default::default()
            },
            Vertex2d {
                pos: right.into(),
                ..Default::default()
            },
            Vertex2d {
                pos: left.into(),
                ..Default::default()
            },
        ]
    }
}

trait VecTools {
    fn limit(&self, max: f32) -> Self;
}

impl VecTools for Vec2 {
    fn limit(&self, max: f32) -> Self {
        if self.norm_squared() > max * max {
            self.normalize().scale(max)
        } else {
            *self
        }
    }
}