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

extern crate nalgebra;
extern crate collisions;

use nalgebra::{Vector2, Vector3, Norm};

use collisions::collidable::{Collidable, CollidableShape};

pub struct Physical
{
    pub collidable: Collidable,
    pub velocity: Vector2<f32>,
    pub angular_velocity: f32,
    pub centre_of_mass: Vector2<f32>,
    pub restitution: f32,
    pub inertia: f32,
}

pub struct Resolution
{
    pub trans_impulse_a: Vector2<f32>,
    pub ang_impulse_a: f32,
    pub trans_impulse_b: Vector2<f32>,
    pub ang_impulse_b: f32,
}

pub fn resolve_poly_with_any(poly: &Physical, other: &Physical, face: usize, support: Vector2<f32>) -> Resolution
{
    let relative_velocity = other.velocity - poly.velocity;
    let normal = nalgebra::normalize(&Vector2::new(poly.collidable.normx[face], poly.collidable.normy[face]));

    let velocity_along_normal = nalgebra::dot(&relative_velocity, &normal);
    if (velocity_along_normal <= 0.0) // if velocities are towards each other
    {
        let e = poly.restitution * other.restitution;
        let j = -(e + 1.0) * velocity_along_normal;

        let poly_impulse = -normal * j * (other.inertia / (other.inertia + poly.inertia));
        let poly_h = poly.centre_of_mass - support;

        if poly_impulse.norm() > 0.0 && poly_h.norm() > 0.0
        {
            let poly_trans_impulse = poly_impulse * nalgebra::dot(&nalgebra::normalize(&poly_h), &nalgebra::normalize(&poly_impulse));

            let poly_ha = nalgebra::normalize(&Vector3::new(poly_h.x, poly_h.y, 0.0));
            let poly_ia = nalgebra::normalize(&Vector3::new(poly_impulse.x, poly_impulse.y, 0.0));
            let poly_ang_impulse = -(nalgebra::cross(&poly_ha, &poly_ia)).z / poly_impulse.norm() / poly_h.norm();

            let other_impulse = -normal * j * (poly.inertia / (poly.inertia + other.inertia));
            let other_h = other.centre_of_mass - support;

            if other_impulse.norm() > 0.0 && other_h.norm() > 0.0
            {
                let other_trans_impulse = other_impulse * nalgebra::dot(&nalgebra::normalize(&other_h), &nalgebra::normalize(&other_impulse));

                let other_ha = nalgebra::normalize(&Vector3::new(other_h.x, other_h.y, 0.0));
                let other_ia = nalgebra::normalize(&Vector3::new(other_impulse.x, other_impulse.y, 0.0));
                let other_ang_impulse = -(nalgebra::cross(&other_ha, &other_ia)).z / other_impulse.norm() / other_h.norm();

                return Resolution {
                    trans_impulse_a: poly_trans_impulse,
                    ang_impulse_a: poly_ang_impulse,
                    trans_impulse_b: other_trans_impulse,
                    ang_impulse_b: other_ang_impulse,
                };
            }
        }
    }

    return Resolution {
        trans_impulse_a: nalgebra::zero(),
        ang_impulse_a: 0.0,
        trans_impulse_b: nalgebra::zero(),
        ang_impulse_b: 0.0,
    }
}

pub fn resolve_circle_with_circle(a: &Physical, b: &Physical, face: usize, support: Vector2<f32>) -> Resolution
{
    let relative_velocity = b.velocity - a.velocity;
    let normal = nalgebra::normalize(&(b.centre_of_mass - a.centre_of_mass));

    let velocity_along_normal = nalgebra::dot(&relative_velocity, &normal);
    if (velocity_along_normal <= 0.0) // if velocities are towards each other
    {
        let e = a.restitution * b.restitution;
        let j = -(e + 1.0) * velocity_along_normal;

        let a_impulse = -normal * j * (b.inertia / (b.inertia + a.inertia));
        let a_h = a.centre_of_mass - support;
        let a_trans_impulse = a_impulse * nalgebra::dot(&nalgebra::normalize(&a_h), &nalgebra::normalize(&a_impulse));

        let b_impulse = -normal * j * (a.inertia / (a.inertia + b.inertia));
        let b_h = b.centre_of_mass - support;
        let b_trans_impulse = b_impulse * nalgebra::dot(&nalgebra::normalize(&b_h), &nalgebra::normalize(&b_impulse));

        Resolution {
            trans_impulse_a: a_trans_impulse,
            ang_impulse_a: 0.0,
            trans_impulse_b: b_trans_impulse,
            ang_impulse_b: 0.0,
        }
    }
    else
    {
        Resolution {
            trans_impulse_a: nalgebra::zero(),
            ang_impulse_a: 0.0,
            trans_impulse_b: nalgebra::zero(),
            ang_impulse_b: 0.0,
        }
    }
}