rust/hwphysics/src/collision.rs
author alfadur
Fri, 09 Nov 2018 03:36:21 +0300
changeset 14179 abbb74b9cb62
parent 14178 a4c17cfaa4c9
child 14702 29dbe9ce8b7d
permissions -rw-r--r--
generalize adding data to World
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
     1
use std::{
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
     2
    ops::RangeInclusive
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
     3
};
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
     4
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
     5
use crate::{
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
     6
    common::{GearId, GearData, GearDataProcessor},
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
     7
    physics::PhysicsData,
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
     8
    grid::Grid
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
     9
};
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    10
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    11
use fpnum::*;
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    12
use integral_geometry::{
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    13
    Point, Size, GridIndex
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    14
};
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    15
use land2d::Land2D;
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    16
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    17
pub fn fppoint_round(point: &FPPoint) -> Point {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    18
    Point::new(point.x().round() as i32, point.y().round() as i32)
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    19
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    20
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    21
#[derive(PartialEq, Eq, Clone, Copy, Debug)]
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    22
pub struct CircleBounds {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    23
    pub center: FPPoint,
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    24
    pub radius: FPNum
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    25
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    26
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    27
impl CircleBounds {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    28
    pub fn intersects(&self, other: &CircleBounds) -> bool {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    29
        (other.center - self.center).is_in_range(self.radius + other.radius)
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    30
    }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    31
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    32
    pub fn rows(&self) -> impl Iterator<Item = (usize, RangeInclusive<usize>)> {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    33
        let radius = self.radius.abs_round() as usize;
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    34
        let center = Point::from_fppoint(&self.center);
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    35
        (center.y as usize - radius..=center.y as usize + radius)
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    36
            .map(move |row| (row, center.x as usize - radius..=center.x as usize + radius))
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    37
    }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    38
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    39
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    40
#[derive(PartialEq, Eq, Clone, Copy, Debug)]
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    41
pub struct CollisionData {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    42
    pub bounds: CircleBounds
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    43
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    44
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    45
impl GearData for CollisionData {}
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    46
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    47
#[derive(PartialEq, Eq, Clone, Copy, Debug)]
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    48
pub struct ContactData {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    49
    pub elasticity: FPNum,
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    50
    pub friction: FPNum
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    51
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    52
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    53
impl GearData for ContactData {}
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    54
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    55
struct EnabledCollisionsCollection {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    56
    gear_ids: Vec<GearId>,
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    57
    collisions: Vec<CollisionData>
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    58
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    59
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    60
impl EnabledCollisionsCollection {
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    61
    fn new() -> Self {
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    62
        Self {
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    63
            gear_ids: Vec::new(),
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    64
            collisions: Vec::new()
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    65
        }
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    66
    }
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
    67
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    68
    fn push(&mut self, gear_id: GearId, collision: CollisionData) {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    69
        self.gear_ids.push(gear_id);
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    70
        self.collisions.push(collision);
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    71
    }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    72
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    73
    fn iter(&self) -> impl Iterator<Item = (GearId, &CollisionData)> {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    74
        self.gear_ids.iter().cloned().zip(self.collisions.iter())
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    75
    }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    76
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    77
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    78
pub struct CollisionProcessor {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    79
    grid: Grid,
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    80
    enabled_collisions: EnabledCollisionsCollection,
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    81
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    82
    detected_collisions: DetectedCollisions,
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    83
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    84
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    85
pub struct DetectedCollisions {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    86
    pub pairs: Vec<(GearId, GearId)>,
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    87
    pub positions: Vec<Point>
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    88
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    89
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    90
impl DetectedCollisions {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    91
    pub fn new(capacity: usize) -> Self {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    92
        Self {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    93
            pairs: Vec::with_capacity(capacity),
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    94
            positions: Vec::with_capacity(capacity),
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    95
        }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    96
    }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    97
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    98
    pub fn push(&mut self, contact_gear_id1: GearId, contact_gear_id2: GearId, position: &FPPoint) {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
    99
        self.pairs.push((contact_gear_id1, contact_gear_id2));
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   100
        self.positions.push(fppoint_round(&position));
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   101
    }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   102
}
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   103
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   104
impl CollisionProcessor {
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   105
    pub fn new(size: Size) -> Self {
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   106
        Self {
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   107
            grid: Grid::new(size),
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   108
            enabled_collisions: EnabledCollisionsCollection::new(),
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   109
            detected_collisions: DetectedCollisions::new(0)
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   110
        }
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   111
    }
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   112
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   113
    pub fn process(&mut self, land: &Land2D<u32>, updates: &crate::physics::PositionUpdates) {
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   114
        self.grid.check_collisions(&mut self.detected_collisions);
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   115
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   116
        for (gear_id, collision) in self.enabled_collisions.iter() {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   117
            if collision.bounds.rows().any(|(y, r)| (&land[y][r]).iter().any(|v| *v != 0)) {
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   118
                self.detected_collisions.push(gear_id, 0, &collision.bounds.center)
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   119
            }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   120
        }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   121
    }
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   122
}
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   123
14179
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   124
impl GearDataProcessor<CollisionData> for CollisionProcessor {
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   125
    fn add(&mut self, gear_id: GearId, gear_data: CollisionData) {
abbb74b9cb62 generalize adding data to World
alfadur
parents: 14178
diff changeset
   126
        self.grid.insert_static(gear_id, &gear_data.bounds);
14178
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   127
    }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents:
diff changeset
   128
}