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

use crate::Vect;
use std::mem;

/// Base is useful for for simulating car movement though method is so precise that you can chain

/// lot of bases and make totally smooth moving centipede

#[derive(Copy, Clone, Debug)]
pub struct  Base {
    pub a: Vect,
    pub b: Vect,
    l: f32,
}

impl Base {
    pub const ZERO: Self = Self { a: Vect::ZERO, b: Vect::ZERO, l: 0f32 };

    #[inline]
    pub fn new(a: Vect, b: Vect) -> Self {
        Self{ a, b, l: (a - b).len() }
    }

    /// returns base rotation

    #[inline]
    pub fn rotation(&self) -> f32 {
        (self.a - self.b).ang()
    }

    /// returns base center

    #[inline]
    pub fn center(&self) -> Vect {
        self.a + (self.b - self.a)/2f32
    }


    fn mv(&mut self, delta: Vect) {
        self.a += delta;
        self.b += delta;
    }

    /// rotates base around pivot point

    #[inline]
    pub fn rotate_around(&mut self, delta: f32, point: Vect) {
        self.mv(point.inverted());
        self.a = self.a.rot(delta);
        self.b = self.b.rot(delta);
        self.mv(point);
    }

    /// rotates base around its center

    #[inline]
    pub fn rotate(&mut self, delta: f32) {
        self.rotate_around(delta, self.center());
    }

    /// returns direction base is heading

    #[inline]
    pub fn dir(&self) -> Vect {
        self.a - self.b
    }

    /// sets the base position

    #[inline]
    pub fn set_pos(&mut self, pos: Vect) {
        let d = self.dir()/2f32;
        self.a = pos + d;
        self.b = pos - d;
    }

    /// sets the base rotation

    #[inline]
    pub fn set_rot(&mut self, rot: f32) {
        let d = Vect::rad(rot ,self.l/2f32);
        let c = self.center();
        self.a = d + c;
        self.b = d.inverted() + c;
    }

    /// tha main feature if base. Pulling base creates smooth movement where beck point is dragged by

    /// front point. It returns the movement if back point witch you can use to move another base

    /// "connected" to this one

    #[inline]
    pub fn pull(&mut self, mut force: Vect) -> Vect {
        let mut l = force.len();

        if l == 0.0 {
            return Vect::ZERO
        }

        let mut extra = Vect::ZERO;

        if l > self.l {
            extra = force.norm() * self.l;
            self.b = self.a + extra;
            mem::swap(&mut self.a, &mut self.b);

            force -= extra;
            l -= self.l
        }

        let dir = self.dir();

        let projected = Vect::rad(dir.ang_to(force), l);
        let back_move = self.l - (self.l*self.l-projected.y*projected.y).sqrt() + projected.x;
        let back_force = dir.norm() * back_move;

        self.a += force;
        self.b += back_force;

        back_force + extra
    }
}

#[cfg(test)]
mod tests {
    use crate::math::base::Base;
    use crate::Vect;
    #[test]
    fn pull_test() {
        let mut base = Base::new(vect!(0, 0), vect!(0, 100));
        base.pull(vect!(1000, 0));
        println!("{:?}", base);
    }
}