1use dynamis_model::math::{add, dot, length, mul, negate, normalize, sub};
2use dynamis_model::{BodyDesc, BodyHandle, ColliderDesc, QueryFilter, Shape};
3use dynamis_world::{QueryHandle, QueryHit, World};
4
5const SKIN: f32 = 0.05;
6
7pub struct CharacterDesc {
8 pub radius: f32,
9 pub half_height: f32,
10 pub step_height: f32,
11 pub slope_limit: f32,
12 pub max_speed: f32,
13 pub jump_speed: f32,
14}
15
16impl Default for CharacterDesc {
17 fn default() -> Self {
18 Self {
19 radius: 0.4,
20 half_height: 0.5,
21 step_height: 0.3,
22 slope_limit: 50.0_f32.to_radians(),
23 max_speed: 4.0,
24 jump_speed: 5.0,
25 }
26 }
27}
28
29struct Cast {
30 forward: Option<QueryHandle>,
31 forward_low: Option<QueryHandle>,
32 lifted_forward: Option<QueryHandle>,
33 down: Option<QueryHandle>,
34 ceiling: Option<QueryHandle>,
35 down_length: f32,
36}
37
38impl Cast {
39 fn handles(&self) -> [Option<QueryHandle>; 5] {
40 [
41 self.forward,
42 self.forward_low,
43 self.lifted_forward,
44 self.down,
45 self.ceiling,
46 ]
47 }
48}
49
50struct Sweeps {
51 forward: Option<QueryHit>,
52 lifted_forward: Option<QueryHit>,
53 down: Option<QueryHit>,
54 ceiling: Option<QueryHit>,
55 down_length: f32,
56}
57
58pub struct Character {
59 body: BodyHandle,
60 desc: CharacterDesc,
61 vertical: f32,
62 grounded: bool,
63 position: [f32; 3],
64 cast: Option<Cast>,
65}
66
67impl Character {
68 pub fn spawn(world: &mut World, position: [f32; 3], desc: CharacterDesc) -> Self {
69 assert!(desc.radius > 0.0, "character radius must be positive");
70 assert!(
71 desc.half_height >= 0.0,
72 "character half height must be non-negative"
73 );
74 assert!(
75 desc.step_height >= 0.0,
76 "character step height must be non-negative"
77 );
78 assert!(desc.max_speed > 0.0, "character max speed must be positive");
79 assert!(
80 desc.jump_speed >= 0.0,
81 "character jump speed must be non-negative"
82 );
83 assert!(
84 (0.0..=std::f32::consts::FRAC_PI_2).contains(&desc.slope_limit),
85 "character slope limit must be within [0, pi/2]"
86 );
87 let body = world.spawn(
88 BodyDesc::new(ColliderDesc::new(Shape::capsule(
89 desc.radius,
90 desc.half_height,
91 )))
92 .position(position)
93 .kinematic(true),
94 );
95 Self {
96 body,
97 desc,
98 vertical: 0.0,
99 grounded: true,
100 position,
101 cast: None,
102 }
103 }
104
105 pub fn step(&mut self, world: &mut World, dt: f32, move_dir: [f32; 3], jump: bool) {
106 assert!(dt > 0.0, "character dt must be positive");
107 let up = up_of(world.config().gravity);
108 let gravity_magnitude = length(world.config().gravity);
109 let horizontal = if length(move_dir) > 0.0 {
110 mul(normalize(move_dir), self.desc.max_speed)
111 } else {
112 [0.0; 3]
113 };
114 let sweeps = self.take_sweeps(world, up);
115 let mut jumped = false;
116 if self.grounded {
117 self.vertical = 0.0;
118 if jump && gravity_magnitude > 0.0 {
119 self.vertical = self.desc.jump_speed;
120 jumped = true;
121 }
122 } else {
123 self.vertical -= gravity_magnitude * dt;
124 }
125 let vertical = self.vertical;
126 let mut target = self.position;
127 let mut grounded = false;
128 let mut vertical_after = vertical;
129 let mut cpu_horizontal = horizontal;
130 let mut press_dir = [0.0; 3];
131 let slope_cos = self.desc.slope_limit.cos();
132 if let Some(sweeps) = sweeps {
133 if !jumped && let Some(landing) = sweeps.down {
134 target = add(
135 target,
136 mul(
137 up,
138 -(landing.distance - SKIN).clamp(0.0, sweeps.down_length),
139 ),
140 );
141 let surface = negate(landing.normal);
142 grounded = dot(surface, up) > slope_cos;
143 vertical_after = if grounded { 0.0 } else { vertical };
144 }
145 if sweeps.forward.is_some() {
146 match sweeps.lifted_forward {
147 None => {
148 target = add(self.position, mul(up, self.desc.step_height));
149 grounded = false;
150 vertical_after = vertical;
151 }
152 Some(_) => {
153 cpu_horizontal = [0.0; 3];
154 press_dir = normalize(horizontal);
155 }
156 }
157 }
158 if sweeps.ceiling.is_some() && vertical_after > 0.0 {
159 vertical_after = 0.0;
160 }
161 }
162 let velocity = add(horizontal, mul(up, vertical_after));
163 let moved = add(mul(cpu_horizontal, dt), mul(up, vertical_after * dt));
164 self.position = add(target, moved);
165 self.grounded = grounded;
166 self.vertical = vertical_after;
167 let press = mul(press_dir, SKIN * 0.5);
168 let pre_move = sub(add(self.position, press), mul(velocity, dt));
169 world.set_position(self.body, pre_move);
170 world.set_velocity(self.body, velocity);
171 self.cast = Some(self.cast_sweeps(world, dt, up, horizontal, vertical_after, jumped));
172 }
173
174 pub fn body(&self) -> BodyHandle {
175 self.body
176 }
177
178 pub fn position(&self) -> [f32; 3] {
179 self.position
180 }
181
182 pub fn grounded(&self) -> bool {
183 self.grounded
184 }
185
186 pub fn vertical_speed(&self) -> f32 {
187 self.vertical
188 }
189
190 fn take_sweeps(&mut self, world: &mut World, up: [f32; 3]) -> Option<Sweeps> {
191 let cast = self.cast.take()?;
192 world.poll();
193 if cast
194 .handles()
195 .into_iter()
196 .flatten()
197 .any(|handle| !world.query_ready(handle))
198 {
199 world.resolve_queries();
200 for handle in cast.handles().into_iter().flatten() {
201 world.wait_query(handle);
202 }
203 }
204 let forward = cast.forward.and_then(|handle| world.query_hit(handle));
205 let forward_low = cast.forward_low.and_then(|handle| world.query_hit(handle));
206 Some(Sweeps {
207 forward: pick_forward(forward, forward_low, up),
208 lifted_forward: cast
209 .lifted_forward
210 .and_then(|handle| world.query_hit(handle)),
211 down: cast.down.and_then(|handle| world.query_hit(handle)),
212 ceiling: cast.ceiling.and_then(|handle| world.query_hit(handle)),
213 down_length: cast.down_length,
214 })
215 }
216
217 fn cast_sweeps(
218 &self,
219 world: &mut World,
220 dt: f32,
221 up: [f32; 3],
222 horizontal: [f32; 3],
223 vertical: f32,
224 jumped: bool,
225 ) -> Cast {
226 let position = self.position;
227 let probe = Shape::sphere(self.desc.radius);
228 let identity = [0.0, 0.0, 0.0, 1.0];
229 let bottom = add(position, mul(up, -self.desc.half_height));
230 let top = add(position, mul(up, self.desc.half_height));
231 let filter = QueryFilter {
232 exclude: Some(self.body),
233 max_hits: 1,
234 ..QueryFilter::default()
235 };
236 let horizontal_speed = length(horizontal);
237 let forward_length = horizontal_speed * dt + SKIN;
238 let down_length = SKIN + (vertical.abs() + horizontal_speed) * dt;
239 let up_length = SKIN + vertical.max(0.0) * dt;
240 let forward_dir = if horizontal_speed > 0.0 {
241 Some(normalize(horizontal))
242 } else {
243 None
244 };
245 let forward = forward_dir.map(|direction| {
246 world.sweep_query(
247 &probe,
248 identity,
249 position,
250 direction,
251 forward_length,
252 &filter,
253 )
254 });
255 let forward_low = forward_dir.map(|direction| {
256 world.sweep_query(&probe, identity, bottom, direction, forward_length, &filter)
257 });
258 let lifted = add(position, mul(up, self.desc.step_height));
259 let lifted_forward = forward_dir.map(|direction| {
260 world.sweep_query(&probe, identity, lifted, direction, forward_length, &filter)
261 });
262 let down = if !jumped {
263 Some(world.sweep_query(&probe, identity, bottom, negate(up), down_length, &filter))
264 } else {
265 None
266 };
267 let ceiling = if vertical > 0.0 {
268 Some(world.sweep_query(&probe, identity, top, up, up_length, &filter))
269 } else {
270 None
271 };
272 Cast {
273 forward,
274 forward_low,
275 lifted_forward,
276 down,
277 ceiling,
278 down_length,
279 }
280 }
281}
282
283fn up_of(gravity: [f32; 3]) -> [f32; 3] {
284 let magnitude = length(gravity);
285 if magnitude > 0.0 {
286 mul(gravity, -1.0 / magnitude)
287 } else {
288 [0.0, 1.0, 0.0]
289 }
290}
291
292fn pick_forward(
293 first: Option<QueryHit>,
294 second: Option<QueryHit>,
295 up: [f32; 3],
296) -> Option<QueryHit> {
297 let floor_like = |hit: &QueryHit| dot(hit.normal, up) < -0.5;
298 match (first, second) {
299 (Some(a), Some(b)) => {
300 if floor_like(&a) && floor_like(&b) {
301 None
302 } else if floor_like(&a) {
303 Some(b)
304 } else if floor_like(&b) {
305 Some(a)
306 } else {
307 Some(if a.distance <= b.distance { a } else { b })
308 }
309 }
310 (Some(a), None) => {
311 if floor_like(&a) {
312 None
313 } else {
314 Some(a)
315 }
316 }
317 (None, Some(b)) => {
318 if floor_like(&b) {
319 None
320 } else {
321 Some(b)
322 }
323 }
324 (None, None) => None,
325 }
326}