rocketsim 0.2.0

Simulate Rocket League games at maximum efficiency
Documentation 
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

use glam::{Affine3A, Vec3A};

use super::solver::{Epa2, EpaStatus, Gjk, GjkStatus, MinkowskiDiff};
use crate::bullet::collision::shapes::collision_shape::CollisionShapes;

pub struct GjkEpa2Result {
    pub witnesses: [Vec3A; 2],
    pub normal: Vec3A,
    pub penetrating: bool,
}

fn penetration(
    shape_a: &CollisionShapes,
    trans_a: &Affine3A,
    shape_b: &CollisionShapes,
    trans_b: &Affine3A,
    guess: Vec3A,
) -> Option<GjkEpa2Result> {
    let shape = MinkowskiDiff::new(shape_a, *trans_a, shape_b, *trans_b);

    let mut gjk = Gjk::new(&shape);
    match gjk.evaluate::<true>(-guess) {
        GjkStatus::Failed | GjkStatus::Valid => return None,
        GjkStatus::Inside => {}
    }

    let mut epa = Epa2::new();
    let status: EpaStatus = epa.evaluate::<true>(gjk, guess);
    debug_assert!(status == EpaStatus::Valid || status == EpaStatus::AccuracyReached);

    let mut w0 = Vec3A::ZERO;
    for i in 0..epa.result.rank {
        w0 += shape.support0::<true>(epa.sv_store[epa.result.c[i]].d) * epa.result.p[i];
    }

    // w0 is already in world space: MinkowskiDiff::support0 transforms the
    // local support point by trans_a. Do not transform it a second time.
    // This matches the distance fallback below, which returns world witnesses.
    Some(GjkEpa2Result {
        witnesses: [w0, w0 - epa.normal * epa.depth],
        normal: -epa.normal,
        penetrating: true,
    })
}

fn distance(
    shape_a: &CollisionShapes,
    trans_a: &Affine3A,
    shape_b: &CollisionShapes,
    trans_b: &Affine3A,
    guess: Vec3A,
) -> Option<GjkEpa2Result> {
    let shape = MinkowskiDiff::new(shape_a, *trans_a, shape_b, *trans_b);

    let mut gjk = Gjk::new(&shape);
    match gjk.evaluate::<false>(guess) {
        GjkStatus::Inside | GjkStatus::Failed => return None,
        GjkStatus::Valid => {}
    }

    let simplex = gjk.simplex();
    if simplex.rank == 0 {
        return None;
    }

    let mut w0 = Vec3A::ZERO;
    let mut w1 = Vec3A::ZERO;

    for (i, sv_d) in gjk.simplex_d().enumerate() {
        let weight = simplex.p[i];
        if weight != 0.0 {
            w0 += shape.support0::<false>(sv_d) * weight;
            w1 += shape.support1::<false>(-sv_d) * weight;
        }
    }

    // Bullet's Distance normal is w0-w1 (shape B toward shape A). Keep the
    // same direction in world space.
    Some(GjkEpa2Result {
        witnesses: [w0, w1],
        normal: (w0 - w1).normalize_or_zero(),
        penetrating: false,
    })
}

pub fn calc_pen_depth(
    shape_a: &CollisionShapes,
    shape_b: &CollisionShapes,
    trans_a: &Affine3A,
    trans_b: &Affine3A,
) -> Option<GjkEpa2Result> {
    let guess_vectors = [
        (trans_b.translation - trans_a.translation).normalize_or_zero(),
        (trans_a.translation - trans_b.translation).normalize_or_zero(),
        Vec3A::Z,
        Vec3A::Y,
        Vec3A::X,
        Vec3A::ONE.with_z(0.0),
        Vec3A::ONE,
        Vec3A::ONE.with_x(0.0),
        Vec3A::ONE.with_y(0.0),
    ];

    for guess in guess_vectors {
        let results = penetration(shape_a, trans_a, shape_b, trans_b, guess);
        if results.is_some() {
            return results;
        }

        let results = distance(shape_a, trans_a, shape_b, trans_b, guess);
        if results.is_some() {
            return results;
        }
    }

    None
}