ferravex_dynamics/
character.rs

1use crate::{math::*, *};
2
3/// A compact kinematic sphere controller, with positive Y as its up axis.
4/// The caller supplies displacement (including gravity) and owns velocity.
5#[derive(Clone, Copy, Debug)]
6pub struct CharacterSettings {
7    pub radius: f32,
8    pub skin: f32,
9    pub ground_snap: f32,
10    pub step_height: f32,
11    /// A surface is walkable if its upward normal component meets this value.
12    pub min_ground_normal: f32,
13    pub max_iterations: u32,
14}
15impl Default for CharacterSettings {
16    fn default() -> Self {
17        Self {
18            radius: 0.4,
19            skin: 0.002,
20            ground_snap: 0.08,
21            step_height: 0.25,
22            min_ground_normal: 0.7,
23            max_iterations: 5,
24        }
25    }
26}
27#[derive(Clone, Copy, Debug, PartialEq)]
28pub struct CharacterMotion<const D: usize> {
29    pub position: [f32; D],
30    pub translation: [f32; D],
31    pub grounded: bool,
32    pub stepped: bool,
33    pub blocked: bool,
34}
35impl<const D: usize> PhysicsWorld<D> {
36    /// Sweep/slide a sphere through the queried world, with low-step climbing and
37    /// downward ground snap. This does not insert/move a body or apply impulses.
38    /// Exclude the character's body with the filter if it is also in this world.
39    /// Slopes use collider normals; currently terrain is spheres and AABBs only.
40    /// Starting penetrations are not repaired: spawn outside solid colliders.
41    pub fn move_character(
42        &self,
43        origin: [f32; D],
44        displacement: [f32; D],
45        settings: CharacterSettings,
46        filter: QueryFilter,
47    ) -> Result<CharacterMotion<D>, PhysicsError> {
48        if !valid(origin) || !valid(displacement) {
49            return Err(PhysicsError::InvalidBody);
50        }
51        if !Shape::<D>::ball(settings.radius).valid()
52            || !settings.skin.is_finite()
53            || !(1e-5..=0.1).contains(&settings.skin)
54            || !settings.ground_snap.is_finite()
55            || !(0. ..=10.).contains(&settings.ground_snap)
56            || !settings.step_height.is_finite()
57            || !(0. ..=10.).contains(&settings.step_height)
58            || !settings.min_ground_normal.is_finite()
59            || !(0.01..=1.).contains(&settings.min_ground_normal)
60            || !(1..=16).contains(&settings.max_iterations)
61        {
62            return Err(PhysicsError::InvalidConfiguration);
63        }
64        let down = unit(1, -1.);
65        let up = unit(1, 1.);
66        let mut position = origin;
67        let grounded_at_start = self
68            .cast_ball(
69                origin,
70                settings.radius,
71                down,
72                settings.ground_snap + settings.skin,
73                filter,
74            )
75            .is_some_and(|hit| hit.normal[1] >= settings.min_ground_normal);
76        let mut grounded = false;
77        let mut stepped = false;
78        let mut blocked = false;
79        let mut remaining = displacement;
80        for _ in 0..settings.max_iterations {
81            let distance = length(remaining);
82            if distance < 1e-7 {
83                break;
84            }
85            let direction = mul(remaining, distance.recip());
86            let Some(hit) = self.cast_ball(
87                position,
88                settings.radius,
89                direction,
90                distance + settings.skin,
91                filter,
92            ) else {
93                position = add(position, remaining);
94                break;
95            };
96            // Ignore a surface beyond the requested move but inside the skin.
97            if hit.distance > distance + settings.skin * 0.5 {
98                position = add(position, remaining);
99                break;
100            }
101            let advance = (hit.distance - settings.skin).max(0.).min(distance);
102            position = add(position, mul(direction, advance));
103            remaining = mul(remaining, (distance - advance) / distance);
104            grounded |= hit.normal[1] >= settings.min_ground_normal;
105            blocked = true;
106            if grounded_at_start
107                && displacement[1] <= 0.
108                && settings.step_height > 0.
109                && hit.normal[1] < settings.min_ground_normal
110            {
111                let horizontal = std::array::from_fn(|i| if i == 1 { 0. } else { remaining[i] });
112                let distance = length(horizontal);
113                if distance > 1e-6
114                    && self
115                        .cast_ball(
116                            position,
117                            settings.radius,
118                            up,
119                            settings.step_height + settings.skin,
120                            filter,
121                        )
122                        .is_none()
123                {
124                    let raised = add(position, mul(up, settings.step_height));
125                    if self
126                        .cast_ball(
127                            raised,
128                            settings.radius,
129                            mul(horizontal, distance.recip()),
130                            distance + settings.skin,
131                            filter,
132                        )
133                        .is_none()
134                    {
135                        let across = add(raised, horizontal);
136                        if let Some(landing) = self.cast_ball(
137                            across,
138                            settings.radius,
139                            down,
140                            settings.step_height + settings.ground_snap,
141                            filter,
142                        ) && landing.normal[1] >= settings.min_ground_normal
143                        {
144                            position = add(
145                                across,
146                                mul(down, (landing.distance - settings.skin).max(0.)),
147                            );
148                            grounded = true;
149                            stepped = true;
150                            break;
151                        }
152                    }
153                }
154            }
155            let mut normal = hit.normal;
156            if normal[1] > 0. && normal[1] < settings.min_ground_normal {
157                // Steep faces block horizontal intent without turning it uphill.
158                normal[1] = 0.;
159                normal = normalize(normal).unwrap_or(hit.normal);
160            }
161            let into = dot(remaining, normal);
162            if into < 0. {
163                remaining = sub(remaining, mul(normal, into));
164            }
165            if advance < 1e-7 && into >= -1e-7 {
166                break;
167            }
168        }
169        if displacement[1] <= 0.
170            && let Some(ground) = self.cast_ball(
171                position,
172                settings.radius,
173                down,
174                settings.ground_snap + settings.skin,
175                filter,
176            )
177            && ground.normal[1] >= settings.min_ground_normal
178        {
179            position = add(
180                position,
181                mul(down, (ground.distance - settings.skin).max(0.)),
182            );
183            grounded = true;
184        }
185        if !valid(position) {
186            return Err(PhysicsError::NumericalLimit);
187        }
188        Ok(CharacterMotion {
189            position,
190            translation: sub(position, origin),
191            grounded,
192            stepped,
193            blocked,
194        })
195    }
196}
ferravex_dynamics/character.rs

ferravex_dynamics/
character.rs
