halley_core/
overlap_physics.rs

1use std::collections::HashMap;
2
3use crate::field::{NodeId, Vec2};
4
5pub const CONTACT_SLOP: f32 = 0.5;
6pub const CONTACT_SKIN: f32 = 1.5;
7pub const MAX_PHYSICS_SPEED: f32 = 1600.0;
8pub const CONTACT_RESTITUTION: f32 = 0.02;
9pub const CONTACT_FRICTION: f32 = 0.22;
10pub const MAX_CONTACT_IMPULSE: f32 = 380.0;
11pub const MAX_POSITION_CORRECTION: f32 = 48.0;
12pub const POSITION_SOLVER_ITERS: usize = 6;
13pub const PHYSICS_REST_EPSILON: f32 = 4.0;
14
15#[allow(clippy::too_many_arguments)]
16pub fn resolve_contact_pair(
17    positions: &mut HashMap<NodeId, Vec2>,
18    velocities: &mut HashMap<NodeId, Vec2>,
19    a: NodeId,
20    b: NodeId,
21    dx: f32,
22    dy: f32,
23    gap_x: f32,
24    gap_y: f32,
25    inv_mass_a: f32,
26    inv_mass_b: f32,
27) {
28    let solve_x = gap_x >= gap_y;
29    let normal = if solve_x {
30        Vec2 {
31            x: if dx.abs() > f32::EPSILON {
32                dx.signum()
33            } else if a.as_u64() < b.as_u64() {
34                -1.0
35            } else {
36                1.0
37            },
38            y: 0.0,
39        }
40    } else {
41        Vec2 {
42            x: 0.0,
43            y: if dy.abs() > f32::EPSILON {
44                dy.signum()
45            } else {
46                1.0
47            },
48        }
49    };
50
51    let penetration = if solve_x {
52        (-gap_x).max(0.0)
53    } else {
54        (-gap_y).max(0.0)
55    };
56    if penetration > 0.0 {
57        let correction = (penetration + CONTACT_SLOP).min(MAX_POSITION_CORRECTION);
58        let total_inv = inv_mass_a + inv_mass_b;
59        if total_inv > 0.0 {
60            let move_a = correction * (inv_mass_a / total_inv);
61            let move_b = correction * (inv_mass_b / total_inv);
62            if let Some(pos) = positions.get_mut(&a) {
63                pos.x -= normal.x * move_a;
64                pos.y -= normal.y * move_a;
65            }
66            if let Some(pos) = positions.get_mut(&b) {
67                pos.x += normal.x * move_b;
68                pos.y += normal.y * move_b;
69            }
70        }
71    }
72
73    let Some(va) = velocities.get(&a).copied() else {
74        return;
75    };
76    let Some(vb) = velocities.get(&b).copied() else {
77        return;
78    };
79    let rel_x = vb.x - va.x;
80    let rel_y = vb.y - va.y;
81    let rel_normal = rel_x * normal.x + rel_y * normal.y;
82    if rel_normal >= 0.0 {
83        return;
84    }
85
86    let total_inv = inv_mass_a + inv_mass_b;
87    if total_inv <= 0.0 {
88        return;
89    }
90
91    let normal_impulse = (-(1.0 + CONTACT_RESTITUTION) * rel_normal / total_inv)
92        .min(MAX_CONTACT_IMPULSE)
93        .max(0.0);
94    let impulse_x = normal.x * normal_impulse;
95    let impulse_y = normal.y * normal_impulse;
96
97    if let Some(vel) = velocities.get_mut(&a) {
98        vel.x -= impulse_x * inv_mass_a;
99        vel.y -= impulse_y * inv_mass_a;
100    }
101    if let Some(vel) = velocities.get_mut(&b) {
102        vel.x += impulse_x * inv_mass_b;
103        vel.y += impulse_y * inv_mass_b;
104    }
105
106    let tangent_x = rel_x - normal.x * rel_normal;
107    let tangent_y = rel_y - normal.y * rel_normal;
108    let tangent_len = (tangent_x * tangent_x + tangent_y * tangent_y).sqrt();
109    if tangent_len <= f32::EPSILON {
110        return;
111    }
112    let tx = tangent_x / tangent_len;
113    let ty = tangent_y / tangent_len;
114    let rel_tangent = rel_x * tx + rel_y * ty;
115    let friction_impulse = (-rel_tangent / total_inv).clamp(
116        -CONTACT_FRICTION * normal_impulse,
117        CONTACT_FRICTION * normal_impulse,
118    );
119    let friction_x = tx * friction_impulse;
120    let friction_y = ty * friction_impulse;
121
122    if let Some(vel) = velocities.get_mut(&a) {
123        vel.x -= friction_x * inv_mass_a;
124        vel.y -= friction_y * inv_mass_a;
125    }
126    if let Some(vel) = velocities.get_mut(&b) {
127        vel.x += friction_x * inv_mass_b;
128        vel.y += friction_y * inv_mass_b;
129    }
130}
halley_core/overlap_physics.rs

halley_core/
overlap_physics.rs
