rust/hwphysics/src/collision.rs
author alfadur
Tue, 28 Mar 2023 17:53:30 +0300
changeset 15944 ccd458b08113
parent 15828 44b49f255e31
child 15981 5ba4d3a0c3eb
permissions -rw-r--r--
add iteration overhead benchmark
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     1
use std::ops::RangeInclusive;
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     2
15380
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
     3
use crate::{common::GearId, data::GearDataManager, grid::Grid};
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     4
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     5
use fpnum::*;
15828
44b49f255e31 add type safe power of two sizes
alfadur
parents: 15827
diff changeset
     6
use integral_geometry::{Point, PotSize};
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     7
use land2d::Land2D;
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     8
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
     9
pub fn fppoint_round(point: &FPPoint) -> Point {
15270
7446258fab98 add time events
alfadur
parents: 15266
diff changeset
    10
    Point::new(point.x().round(), point.y().round())
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    11
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    12
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    13
#[derive(PartialEq, Eq, Clone, Copy, Debug)]
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    14
pub struct CircleBounds {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    15
    pub center: FPPoint,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    16
    pub radius: FPNum,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    17
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    18
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    19
impl CircleBounds {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    20
    pub fn intersects(&self, other: &CircleBounds) -> bool {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    21
        (other.center - self.center).is_in_range(self.radius + other.radius)
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    22
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    23
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    24
    pub fn rows(&self) -> impl Iterator<Item = (usize, RangeInclusive<usize>)> {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    25
        let radius = self.radius.abs_round() as usize;
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    26
        let center = Point::from_fppoint(&self.center);
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    27
        (center.y as usize - radius..=center.y as usize + radius)
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    28
            .map(move |row| (row, center.x as usize - radius..=center.x as usize + radius))
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    29
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    30
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    31
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    32
#[derive(PartialEq, Eq, Clone, Copy, Debug)]
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    33
pub struct CollisionData {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    34
    pub bounds: CircleBounds,
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
#[derive(PartialEq, Eq, Clone, Copy, Debug)]
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    38
pub struct ContactData {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    39
    pub elasticity: FPNum,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    40
    pub friction: FPNum,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    41
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    42
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    43
struct EnabledCollisionsCollection {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    44
    gear_ids: Vec<GearId>,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    45
    collisions: Vec<CollisionData>,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    46
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    47
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    48
impl EnabledCollisionsCollection {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    49
    fn new() -> Self {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    50
        Self {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    51
            gear_ids: Vec::new(),
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    52
            collisions: Vec::new(),
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    53
        }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    54
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    55
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    56
    fn push(&mut self, gear_id: GearId, collision: CollisionData) {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    57
        self.gear_ids.push(gear_id);
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    58
        self.collisions.push(collision);
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    59
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    60
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    61
    fn iter(&self) -> impl Iterator<Item = (GearId, &CollisionData)> {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    62
        self.gear_ids.iter().cloned().zip(self.collisions.iter())
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    63
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    64
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    65
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    66
pub struct CollisionProcessor {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    67
    grid: Grid,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    68
    enabled_collisions: EnabledCollisionsCollection,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    69
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    70
    detected_collisions: DetectedCollisions,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    71
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    72
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    73
pub struct DetectedCollisions {
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15261
diff changeset
    74
    pub pairs: Vec<(GearId, Option<GearId>)>,
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    75
    pub positions: Vec<Point>,
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    76
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    77
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    78
impl DetectedCollisions {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    79
    pub fn new(capacity: usize) -> Self {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    80
        Self {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    81
            pairs: Vec::with_capacity(capacity),
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    82
            positions: Vec::with_capacity(capacity),
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    83
        }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    84
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    85
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15261
diff changeset
    86
    pub fn push(
24828281c9c5 reserve zero gear id
alfadur
parents: 15261
diff changeset
    87
        &mut self,
24828281c9c5 reserve zero gear id
alfadur
parents: 15261
diff changeset
    88
        contact_gear_id1: GearId,
24828281c9c5 reserve zero gear id
alfadur
parents: 15261
diff changeset
    89
        contact_gear_id2: Option<GearId>,
24828281c9c5 reserve zero gear id
alfadur
parents: 15261
diff changeset
    90
        position: &FPPoint,
24828281c9c5 reserve zero gear id
alfadur
parents: 15261
diff changeset
    91
    ) {
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    92
        self.pairs.push((contact_gear_id1, contact_gear_id2));
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    93
        self.positions.push(fppoint_round(&position));
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
    94
    }
15266
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
    95
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
    96
    pub fn clear(&mut self) {
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
    97
        self.pairs.clear();
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
    98
        self.positions.clear()
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
    99
    }
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   100
}
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   101
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   102
impl CollisionProcessor {
15380
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   103
    pub fn register_components(data: &mut GearDataManager) {
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   104
        data.register::<CollisionData>();
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   105
        data.register::<ContactData>();
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   106
    }
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   107
15828
44b49f255e31 add type safe power of two sizes
alfadur
parents: 15827
diff changeset
   108
    pub fn new(size: PotSize) -> Self {
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   109
        Self {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   110
            grid: Grid::new(size),
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   111
            enabled_collisions: EnabledCollisionsCollection::new(),
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   112
            detected_collisions: DetectedCollisions::new(0),
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
15380
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   116
    pub fn add(&mut self, gear_id: GearId, gear_data: CollisionData) {
15827
64b0a5cead86 use grid only for static gears
alfadur
parents: 15380
diff changeset
   117
        self.grid.insert(gear_id, &gear_data.bounds);
15380
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   118
    }
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   119
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   120
    pub fn remove(&mut self, gear_id: GearId) {
15827
64b0a5cead86 use grid only for static gears
alfadur
parents: 15380
diff changeset
   121
        self.grid.remove(gear_id, None);
15380
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   122
    }
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   123
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   124
    pub fn get(&mut self, gear_id: GearId) -> Option<CollisionData> {
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   125
        None
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   126
    }
6e3e5be8b2e2 update hwphysics motion to use the new system
alfadur
parents: 15282
diff changeset
   127
15266
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
   128
    pub fn process(
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
   129
        &mut self,
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
   130
        land: &Land2D<u32>,
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
   131
        updates: &crate::physics::PositionUpdates,
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
   132
    ) -> &DetectedCollisions {
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
   133
        self.detected_collisions.clear();
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   134
        self.grid.check_collisions(&mut self.detected_collisions);
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   135
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   136
        for (gear_id, collision) in self.enabled_collisions.iter() {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   137
            if collision
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   138
                .bounds
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   139
                .rows()
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   140
                .any(|(y, r)| (&land[y][r]).iter().any(|v| *v != 0))
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   141
            {
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   142
                self.detected_collisions
15263
24828281c9c5 reserve zero gear id
alfadur
parents: 15261
diff changeset
   143
                    .push(gear_id, None, &collision.bounds.center)
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   144
            }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   145
        }
15266
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
   146
b58f98bbc120 clear intermediate result structures between iterations
alfadur
parents: 15263
diff changeset
   147
        &self.detected_collisions
15120
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   148
    }
febccab419b1 Apply dos2unix to rust sources
unc0rr
parents: 14716
diff changeset
   149
}