rust/hwphysics/src/grid.rs
author alfadur
Thu, 25 Jul 2019 21:59:20 +0300
changeset 15273 bfd185ad03e7
parent 15263 24828281c9c5
child 15274 42b710b0f883
permissions -rw-r--r--
use better contact point for inter-gear collision
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     1
use crate::{
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     2
    collision::{fppoint_round, CircleBounds, DetectedCollisions},
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     3
    common::GearId,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     4
};
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     5
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     6
use fpnum::FPPoint;
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     7
use integral_geometry::{GridIndex, Point, Size};
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     8
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     9
struct GridBin {
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    10
    static_refs: Vec<GearId>,
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    11
    static_entries: Vec<CircleBounds>,
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    12
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    13
    dynamic_refs: Vec<GearId>,
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    14
    dynamic_entries: Vec<CircleBounds>,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    15
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    16
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    17
impl GridBin {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    18
    fn new() -> Self {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    19
        Self {
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    20
            static_refs: vec![],
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    21
            static_entries: vec![],
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    22
            dynamic_refs: vec![],
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    23
            dynamic_entries: vec![],
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    24
        }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    25
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    26
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    27
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    28
const GRID_BIN_SIZE: usize = 128;
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    29
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    30
pub struct Grid {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    31
    bins: Vec<GridBin>,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    32
    space_size: Size,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    33
    bins_count: Size,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    34
    index: GridIndex,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    35
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    36
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    37
impl Grid {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    38
    pub fn new(size: Size) -> Self {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    39
        assert!(size.is_power_of_two());
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    40
        let bins_count = Size::new(size.width / GRID_BIN_SIZE, size.height / GRID_BIN_SIZE);
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    41
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    42
        Self {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    43
            bins: (0..bins_count.area()).map(|_| GridBin::new()).collect(),
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    44
            space_size: size,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    45
            bins_count,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    46
            index: Size::square(GRID_BIN_SIZE).to_grid_index(),
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    47
        }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    48
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    49
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    50
    fn bin_index(&self, position: &FPPoint) -> Point {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    51
        self.index.map(fppoint_round(position))
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    52
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    53
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    54
    fn get_bin(&mut self, index: Point) -> &mut GridBin {
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    55
        &mut self.bins[index.x as usize * self.bins_count.width + index.y as usize]
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    56
    }
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    57
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    58
    fn lookup_bin(&mut self, position: &FPPoint) -> &mut GridBin {
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    59
        self.get_bin(self.bin_index(position))
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    60
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    61
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    62
    pub fn insert_static(&mut self, gear_id: GearId, bounds: &CircleBounds) {
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    63
        let bin = self.lookup_bin(&bounds.center);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    64
        bin.static_refs.push(gear_id);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    65
        bin.static_entries.push(*bounds)
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    66
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    67
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    68
    pub fn insert_dynamic(&mut self, gear_id: GearId, bounds: &CircleBounds) {
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    69
        let bin = self.lookup_bin(&bounds.center);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    70
        bin.dynamic_refs.push(gear_id);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    71
        bin.dynamic_entries.push(*bounds);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    72
    }
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    73
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    74
    pub fn update_position(
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    75
        &mut self,
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    76
        gear_id: GearId,
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    77
        old_position: &FPPoint,
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    78
        new_position: &FPPoint,
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    79
    ) {
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    80
        let old_bin_index = self.bin_index(old_position);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    81
        let new_bin_index = self.bin_index(new_position);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    82
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    83
        let old_bin = self.lookup_bin(old_position);
15262
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    84
        if let Some(index) = old_bin.dynamic_refs.iter().position(|id| *id == gear_id) {
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    85
            if old_bin_index == new_bin_index {
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    86
                old_bin.dynamic_entries[index].center = *new_position
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    87
            } else {
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    88
                let bounds = old_bin.dynamic_entries.swap_remove(index);
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    89
                let new_bin = self.get_bin(new_bin_index);
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    90
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    91
                new_bin.dynamic_refs.push(gear_id);
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    92
                new_bin.dynamic_entries.push(CircleBounds {
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    93
                    center: *new_position,
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    94
                    ..bounds
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    95
                });
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    96
            }
d8c4fd911b37 check dynamic collision arrays first
alfadur
parents: 15261
diff changeset
    97
        } else if let Some(index) = old_bin.static_refs.iter().position(|id| *id == gear_id) {
15261
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    98
            let bounds = old_bin.static_entries.swap_remove(index);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
    99
            old_bin.static_refs.swap_remove(index);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   100
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   101
            let new_bin = if old_bin_index == new_bin_index {
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   102
                old_bin
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   103
            } else {
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   104
                self.get_bin(new_bin_index)
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   105
            };
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   106
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   107
            new_bin.dynamic_refs.push(gear_id);
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   108
            new_bin.dynamic_entries.push(CircleBounds {
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   109
                center: *new_position,
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   110
                ..bounds
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   111
            });
501dfa1c8deb update collision grid with position changes
alfadur
parents: 15120
diff changeset
   112
        }
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   113
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   114
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   115
    pub fn check_collisions(&self, collisions: &mut DetectedCollisions) {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   116
        for bin in &self.bins {
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   117
            for (index, bounds) in bin.dynamic_entries.iter().enumerate() {
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   118
                for (other_index, other) in bin.dynamic_entries.iter().enumerate().skip(index + 1) {
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   119
                    if bounds.intersects(other) && bounds != other {
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   120
                        collisions.push(
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   121
                            bin.dynamic_refs[index],
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   122
                            Some(bin.dynamic_refs[other_index]),
15273
bfd185ad03e7 use better contact point for inter-gear collision
alfadur
parents: 15263
diff changeset
   123
                            &((bounds.center + other.center) / 2),
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   124
                        )
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   125
                    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   126
                }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   127
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   128
                for (other_index, other) in bin.static_entries.iter().enumerate() {
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   129
                    if bounds.intersects(other) {
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   130
                        collisions.push(
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   131
                            bin.dynamic_refs[index],
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   132
                            Some(bin.static_refs[other_index]),
15273
bfd185ad03e7 use better contact point for inter-gear collision
alfadur
parents: 15263
diff changeset
   133
                            &((bounds.center + other.center) / 2),
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15262
diff changeset
   134
                        )
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   135
                    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   136
                }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   137
            }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   138
        }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   139
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   140
}