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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153

use super::math::{Vec2, bounding::AxisAlignmentBoundingBox};
use super::shapes::{Shape, Contact};

pub trait Particle {
    fn integrate(&mut self, dt: f32);
}

pub trait ContactResolver {
    fn resolve_overlap(&self, b1: &mut Body, b2: &mut Body);
    fn resolve_velocity(&self, b1: &mut Body, b2: &mut Body);
}

pub struct Body {
    shape: Option<Shape>,
    position: Vec2,
    inverse_mass: f32,
    velocity: Vec2,
    force: Vec2,
    drag_force: (f32, f32),
    restitution: f32,
}

impl Body {
    pub fn new(position: Vec2) -> Body {
        Body {
            shape: None,
            position,
            inverse_mass: 1.0,
            velocity: Vec2::zero(),
            force: Vec2::zero(),
            drag_force: (0.0, 0.0),
            restitution: 1.0,
        }
    }

    pub fn set_position(&mut self, position: Vec2) {
        self.position = position;
    }

    pub fn displace(&mut self, displacement: Vec2) {
        self.position += displacement;
    }

    pub fn position(&self) -> Vec2 {
        self.position
    }

    pub fn set_shape(&mut self, shape: Shape) {
        self.shape = Some(shape)
    }

    pub fn remove_shape(&mut self) {
        self.shape = None;
    }

    pub fn shape(&self) -> Option<&Shape> {
        self.shape.as_ref()
    }

    pub fn set_mass(&mut self, mass: f32) {
        self.inverse_mass = 1.0 / mass;
    }

    pub fn add_mass(&mut self, mass: f32) {
        self.inverse_mass = 1.0 / (1.0 / self.inverse_mass + mass);
    }

    pub fn mass(&self) -> f32 {
        1.0 / self.inverse_mass
    }

    pub fn inverse_mass(&self) -> f32 {
        self.inverse_mass
    }

    pub fn set_velocity(&mut self, velocity: Vec2) {
        self.velocity = velocity
    }

    pub fn add_velocity(&mut self, velocity: Vec2) {
        self.velocity += velocity
    }

    pub fn velocity(&self) -> Vec2 {
        self.velocity
    }

    pub fn set_force(&mut self, force: Vec2) {
        self.force = force;
    }

    pub fn add_force(&mut self, force: Vec2) {
        self.force += force;
    }

    pub fn force(&self) -> Vec2 {
        self.force
    }

    pub fn set_drag_force(&mut self, drag_force: (f32, f32)) {
        self.drag_force = drag_force;
    }

    pub fn drag_force(&self) -> (f32, f32) {
        self.drag_force
    }

    pub fn set_restitution(&mut self, restitution: f32) {
        self.restitution = restitution;
    }

    pub fn restitution(&self) -> f32 {
        self.restitution
    }

    pub fn aabb(&self) -> Option<AxisAlignmentBoundingBox> {
        self.shape.as_ref().map(|shape| AxisAlignmentBoundingBox::new(self.position, shape.half_dimension()))
    }
}

impl Particle for Body {
    fn integrate(&mut self, dt: f32) {
        let real_force = self.force - self.velocity * (self.drag_force.0 + self.drag_force.1 * self.velocity.length());
        self.velocity += real_force * (self.inverse_mass * dt);

        self.displace(self.velocity * dt);
        self.force = Vec2::zero();
    }
}

impl ContactResolver for Contact {
    fn resolve_overlap(&self, b1: &mut Body, b2: &mut Body) {
        let total_inverse_mass = b1.inverse_mass() + b2.inverse_mass();
        let displacement = self.normal() * (self.overlap() / total_inverse_mass);

        b1.displace(displacement * b1.inverse_mass());
        b2.displace(displacement * -b2.inverse_mass());
    }

    fn resolve_velocity(&self, b1: &mut Body, b2: &mut Body) {
        let separating_speed = self.normal() * (b1.velocity() - b2.velocity());
        if separating_speed < 0.0 {
            let new_separating_speed = -separating_speed * b1.restitution() * b2.restitution();
            let delta_speed  = new_separating_speed - separating_speed;

            let total_inverse_mass = b1.inverse_mass() + b2.inverse_mass();
            let impulse = self.normal() * (delta_speed / total_inverse_mass);

            b1.add_velocity(impulse * b1.inverse_mass());
            b2.add_velocity(impulse * -b2.inverse_mass());
        }
    }
}