khora_core/physics/
solver.rs

1// Copyright 2025 eraflo
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7//     http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15//! # Impulse Solver
16//!
17//! Pure mathematical implementation of constraint resolution using impulses.
18
19use super::{BodyType, ContactManifold};
20use crate::math::Vec3;
21
22/// Represents the physical state of a body relevant to impulse resolution.
23#[derive(Debug, Clone, Copy)]
24pub struct VelocityState {
25    /// Linear velocity vector.
26    pub linear_velocity: Vec3,
27    /// Angular velocity vector.
28    pub angular_velocity: Vec3,
29    /// Mass of the body.
30    pub mass: f32,
31    /// Type of the body (Dynamic, Static, Kinematic).
32    pub body_type: BodyType,
33}
34
35/// A mathematical solver for impulse-based constraint resolution.
36pub struct ImpulseSolver {
37    /// Coefficient of restitution (bounciness).
38    pub restitution: f32,
39    /// Percentage of penetration to resolve per frame (Baumgarte stabilization).
40    pub baumgarte_percent: f32,
41    /// Penetration allowance to avoid jitter.
42    pub slop: f32,
43}
44
45impl ImpulseSolver {
46    /// Creates a new `ImpulseSolver` with default physical constants.
47    pub fn new() -> Self {
48        Self {
49            restitution: 0.2,
50            baumgarte_percent: 0.2,
51            slop: 0.01,
52        }
53    }
54
55    /// Resolves the collision between two bodies.
56    ///
57    /// Returns the updated velocity states for both bodies.
58    pub fn resolve(
59        &self,
60        mut a: VelocityState,
61        mut b: VelocityState,
62        manifold: &ContactManifold,
63    ) -> (VelocityState, VelocityState) {
64        if a.body_type == BodyType::Static && b.body_type == BodyType::Static {
65            return (a, b);
66        }
67
68        // 1. Calculate relative velocity in the direction of the collision normal.
69        let rv = b.linear_velocity - a.linear_velocity;
70        let vel_along_normal = rv.dot(manifold.normal);
71
72        // 2. If already separating, no impulse is needed.
73        if vel_along_normal > 0.0 {
74            return (a, b);
75        }
76
77        // 3. Calculate impulse magnitude (j).
78        let inv_mass_a = if a.body_type == BodyType::Dynamic {
79            1.0 / a.mass
80        } else {
81            0.0
82        };
83        let inv_mass_b = if b.body_type == BodyType::Dynamic {
84            1.0 / b.mass
85        } else {
86            0.0
87        };
88        let total_inv_mass = inv_mass_a + inv_mass_b;
89
90        if total_inv_mass <= 0.0 {
91            return (a, b);
92        }
93
94        let mut j = -(1.0 + self.restitution) * vel_along_normal;
95        j /= total_inv_mass;
96
97        // 4. Apply impulse as a change in linear velocity.
98        let impulse = manifold.normal * j;
99        if a.body_type == BodyType::Dynamic {
100            a.linear_velocity = a.linear_velocity - (impulse * inv_mass_a);
101        }
102        if b.body_type == BodyType::Dynamic {
103            b.linear_velocity = b.linear_velocity + (impulse * inv_mass_b);
104        }
105
106        // 5. Positional correction (Linear Projection) using velocity "slack".
107        let correction_mag =
108            (manifold.depth - self.slop).max(0.0) / total_inv_mass * self.baumgarte_percent;
109        let correction = manifold.normal * correction_mag;
110
111        if a.body_type == BodyType::Dynamic {
112            a.linear_velocity = a.linear_velocity - (correction * inv_mass_a);
113        }
114        if b.body_type == BodyType::Dynamic {
115            b.linear_velocity = b.linear_velocity + (correction * inv_mass_b);
116        }
117
118        (a, b)
119    }
120}
121
122impl Default for ImpulseSolver {
123    fn default() -> Self {
124        Self::new()
125    }
126}
khora_core/physics/solver.rs

khora_core/physics/
solver.rs
