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

mod pair;

pub use self::pair::BodyPair;

use math::{Vec2, Cross};
use ::shapes::{Shape, Matter};
use ::world::Transform;

use std::cell::RefCell;

pub type BodyRef = RefCell<Body>;

pub type BodyId = usize;

pub struct Body {
    pub id: BodyId,
    
    pub transform: Transform,
    
    pub velocity: Vec2,
    pub angular_vel: f32,
    
    force: Vec2,
    torque: f32,
    
    pub mass: f32,
    pub inertia: f32,
    
    pub inv_mass: f32,
    pub inv_inertia: f32,
    
    pub shape: Shape,
}

impl Body {
    pub fn new(shape: Shape, density: f32) -> Body {
        let (mass, inertia) = shape.mass_and_inertia(density);
        
        let inv_mass = if mass != 0.0 { 1.0 / mass } else { 0.0f32 };
        let inv_inertia = if inertia != 0.0 { 1.0 / inertia } else { 0.0f32 };
        
        Body {
            id: BodyId::default(),
            transform: Transform::new(Vec2::ZERO, 0.0),
            velocity: Vec2::ZERO,
            angular_vel: 0.0,
            force: Vec2::ZERO,
            torque: 0.0,
            
            mass,
            inertia,
            inv_mass,
            inv_inertia,
            
            shape,
        }
    }
    
    pub(crate) fn integrate_force(&mut self, dt: f32) {
        // TODO: Make configurable
        const GRAVITY: Vec2 = Vec2 { x: 0.0, y: -9.8 };
        
        if self.is_static() {
            return;
        }
        
        self.velocity += (GRAVITY + self.force * self.inv_mass) * dt;
        self.angular_vel += self.torque * self.inv_inertia * dt;
        
        self.force = Vec2::ZERO;
        self.torque = 0.0;
    }
    
    pub(crate) fn integrate_velocity(&mut self, dt: f32) {
        if self.is_static() {
            return;
        }
        
        self.transform.position += self.velocity * dt;
        
        let new_rotation = self.transform.rotation() + self.angular_vel * dt;
        
        self.transform.set_rotation(new_rotation);
    }
    
    pub(crate) fn update(&mut self, dt: f32) {}
    
    pub fn set_static(&mut self) {
        self.inv_inertia = 0.0;
        self.inertia = 0.0;
        self.mass = 0.0;
        self.inv_mass = 0.0;
    }
    
    pub fn is_static(&self) -> bool {
        self.inv_mass == 0.0 && self.inv_inertia == 0.0
    }
    
    pub fn add_force(&mut self, force: Vec2) {
        self.force += force;
    }
    
    pub fn add_torque(&mut self, torque: f32) {
        self.torque += torque;
    }
    
    pub fn add_force_at_pos(&mut self, force: Vec2, pos: Vec2) {
        self.add_force(force);
        self.add_torque(pos.cross(force));
    }
    
    pub fn add_impulse_at_pos(&mut self, impulse: Vec2, pos: Vec2) {
        self.velocity += impulse * self.inv_mass;
        self.angular_vel += pos.cross(impulse) * self.inv_inertia;
    }
}