1use pfx_core::sim::cosf;
2use rapier3d::control::{
3 CharacterAutostep, CharacterCollision, CharacterLength, KinematicCharacterController,
4};
5use rapier3d::parry::query::{ShapeCastHit, ShapeCastOptions};
6use rapier3d::prelude::{Collider, ColliderHandle, QueryFilter, Ray, RigidBodySet, SharedShape};
7
8use super::convert::{Iso, Quat, Vec3, body_handle, body_id, vec};
9use super::{BodyDesc, BodyId, ColliderDesc, ColliderId, Layers, Locks, Pose, Shape, World};
10
11const WALL: f32 = 0.3;
12const REACH: f32 = 0.05;
13pub const TICKS: u32 = 1000;
14
15#[derive(Clone, Copy, Debug, PartialEq, Eq)]
16pub enum Plane {
17 Xy,
18 Yz,
19}
20
21impl Plane {
22 fn axis(self) -> usize {
23 match self {
24 Self::Xy => 2,
25 Self::Yz => 0,
26 }
27 }
28
29 fn locks(self) -> Locks {
30 match self {
31 Self::Xy => Locks::PLANE_XY,
32 Self::Yz => Locks::PLANE_YZ,
33 }
34 }
35}
36
37#[derive(Clone, Copy, Debug, PartialEq)]
38pub struct CharacterDesc {
39 pub radius: f32,
40 pub height: f32,
41 pub speed: f32,
42 pub acceleration: f32,
43 pub air_acceleration: f32,
44 pub max_climb: f32,
45 pub max_step: f32,
46 pub step_width: f32,
47 pub snap: f32,
48 pub skin: f32,
49 pub jump_speed: f32,
50 pub coyote: u32,
51 pub jump_buffer: u32,
52 pub gravity_scale: f32,
53 pub max_fall: f32,
54 pub mass: f32,
55 pub push: bool,
56 pub ride: bool,
57 pub variable_jump: f32,
58 pub wall_slide: Option<f32>,
59 pub wall_jump: Option<[f32; 2]>,
60 pub plane: Option<Plane>,
61 pub layers: Layers,
62}
63
64impl Default for CharacterDesc {
65 fn default() -> Self {
66 Self {
67 radius: 0.3,
68 height: 1.8,
69 speed: 5.0,
70 acceleration: 60.0,
71 air_acceleration: 20.0,
72 max_climb: 45.0,
73 max_step: 0.3,
74 step_width: 0.1,
75 snap: 0.2,
76 skin: 0.02,
77 jump_speed: 5.0,
78 coyote: 6,
79 jump_buffer: 6,
80 gravity_scale: 1.0,
81 max_fall: 50.0,
82 mass: 80.0,
83 push: true,
84 ride: true,
85 variable_jump: 0.0,
86 wall_slide: None,
87 wall_jump: None,
88 plane: None,
89 layers: Layers::default(),
90 }
91 }
92}
93
94impl CharacterDesc {
95 pub fn flat(plane: Plane) -> Self {
96 Self {
97 plane: Some(plane),
98 ..Self::default()
99 }
100 }
101
102 pub fn check(&self) -> Result<(), String> {
103 let positive = [
104 ("radius", self.radius),
105 ("height", self.height),
106 ("skin", self.skin),
107 ];
108 for (name, value) in positive {
109 if !(value.is_finite() && value > 0.0) {
110 return Err(format!("{name} needs a number above 0"));
111 }
112 }
113 let at_least_zero = [
114 ("speed", self.speed),
115 ("acceleration", self.acceleration),
116 ("air_acceleration", self.air_acceleration),
117 ("max_step", self.max_step),
118 ("step_width", self.step_width),
119 ("snap", self.snap),
120 ("jump_speed", self.jump_speed),
121 ("gravity_scale", self.gravity_scale),
122 ("max_fall", self.max_fall),
123 ("mass", self.mass),
124 ];
125 for (name, value) in at_least_zero {
126 if !(value.is_finite() && value >= 0.0) {
127 return Err(format!("{name} needs a number of 0 or more"));
128 }
129 }
130 if self.height < 2.0 * self.radius {
131 return Err(format!(
132 "height {} is less than twice the radius {}",
133 self.height, self.radius
134 ));
135 }
136 if !(self.max_climb.is_finite() && (0.0..90.0).contains(&self.max_climb)) {
137 return Err("max_climb needs degrees from 0 up to 90".to_string());
138 }
139 if self.max_step >= self.height {
140 return Err(format!(
141 "max_step {} is not below the height {}",
142 self.max_step, self.height
143 ));
144 }
145 for (name, ticks) in [("coyote", self.coyote), ("jump_buffer", self.jump_buffer)] {
146 if ticks > TICKS {
147 return Err(format!("{name} needs 0 to {TICKS} ticks"));
148 }
149 }
150 if !(self.variable_jump.is_finite() && (0.0..=1.0).contains(&self.variable_jump)) {
151 return Err("variable_jump needs a number from 0 to 1".to_string());
152 }
153 if let Some(slide) = self.wall_slide
154 && !(slide.is_finite() && slide >= 0.0)
155 {
156 return Err("wall_slide needs a number of 0 or more".to_string());
157 }
158 if let Some(kick) = self.wall_jump
159 && !kick.iter().all(|part| part.is_finite() && *part >= 0.0)
160 {
161 return Err("wall_jump needs two numbers of 0 or more".to_string());
162 }
163 Ok(())
164 }
165
166 fn half_height(&self) -> f32 {
167 (self.height * 0.5 - self.radius).max(0.0)
168 }
169
170 fn collider(&self) -> ColliderDesc {
171 ColliderDesc::new(self.shape())
172 .with_layers(self.layers)
173 .with_density(0.0)
174 }
175
176 fn shape(&self) -> Shape {
177 Shape::Capsule {
178 half_height: self.half_height(),
179 radius: self.radius,
180 }
181 }
182
183 fn controller(&self, up: Vec3, rising: bool) -> KinematicCharacterController {
184 let radians = std::f32::consts::PI / 180.0;
185 KinematicCharacterController {
186 up,
187 offset: CharacterLength::Absolute(self.skin),
188 slide: true,
189 autostep: (self.max_step > 0.0).then_some(CharacterAutostep {
190 max_height: CharacterLength::Absolute(self.max_step),
191 min_width: CharacterLength::Absolute(self.step_width),
192 include_dynamic_bodies: false,
193 }),
194 max_slope_climb_angle: self.max_climb * radians,
195 min_slope_slide_angle: self.max_climb * radians,
196 snap_to_ground: (!rising && self.snap > 0.0)
197 .then_some(CharacterLength::Absolute(self.snap)),
198 normal_nudge_factor: 1.0e-4,
199 }
200 }
201}
202
203#[derive(Clone, Copy, Debug, Default, PartialEq)]
204pub struct Intent {
205 pub direction: [f32; 3],
206 pub jump: bool,
207}
208
209#[derive(Clone, Debug, PartialEq)]
210pub struct Character {
211 desc: CharacterDesc,
212 body: BodyId,
213 collider: ColliderId,
214 center: Vec3,
215 velocity: Vec3,
216 up: Vec3,
217 intent: Intent,
218 held: bool,
219 buffered: Option<u32>,
220 grounded: bool,
221 sliding: bool,
222 since_ground: u32,
223 jumped: bool,
224 rising: bool,
225 wall: Option<Vec3>,
226 since_wall: u32,
227 ground: Option<BodyId>,
228 anchor: f32,
229 ticks: u64,
230}
231
232impl Character {
233 pub fn spawn(world: &mut World, desc: CharacterDesc, feet: [f32; 3]) -> Result<Self, String> {
234 desc.check()?;
235 let feet = vec(feet);
236 let up = up_of(world, feet, desc.plane);
237 let center = feet + up * (desc.height * 0.5);
238 let locks = desc.plane.map_or(Locks::NONE, Plane::locks);
239 let body = world.add_body(
240 BodyDesc::kinematic(Pose::new(center.to_array(), turn(up).to_array()))
241 .with_locks(locks),
242 );
243 let Some(collider) = world.add_collider(body, desc.collider()) else {
244 world.remove_body(body);
245 return Err("the character's capsule did not build".to_string());
246 };
247 let anchor = desc.plane.map_or(0.0, |plane| center[plane.axis()]);
248 Ok(Self {
249 desc,
250 body,
251 collider,
252 center,
253 velocity: Vec3::ZERO,
254 up,
255 intent: Intent::default(),
256 held: false,
257 buffered: None,
258 grounded: false,
259 sliding: false,
260 since_ground: u32::MAX,
261 jumped: false,
262 rising: false,
263 wall: None,
264 since_wall: u32::MAX,
265 ground: None,
266 anchor,
267 ticks: 0,
268 })
269 }
270
271 pub fn drive(&mut self, direction: [f32; 3], jump: bool) {
272 self.intent = Intent { direction, jump };
273 }
274
275 pub fn intent(&self) -> Intent {
276 self.intent
277 }
278
279 pub fn desc(&self) -> &CharacterDesc {
280 &self.desc
281 }
282
283 pub fn set_desc(&mut self, world: &mut World, desc: CharacterDesc) -> Result<(), String> {
284 desc.check()?;
285 if desc.plane != self.desc.plane {
286 return Err("plane cannot change while the character lives".to_string());
287 }
288 if desc.radius != self.desc.radius
289 || desc.height != self.desc.height
290 || desc.layers != self.desc.layers
291 {
292 let feet = self.feet_vec();
293 let collider = world
294 .add_collider(self.body, desc.collider())
295 .ok_or_else(|| "the character's capsule did not build".to_string())?;
296 world.remove_collider(self.collider);
297 self.collider = collider;
298 self.center = feet + self.up * (desc.height * 0.5);
299 world.set_pose(self.body, self.pose());
300 }
301 self.desc = desc;
302 Ok(())
303 }
304
305 pub fn body(&self) -> BodyId {
306 self.body
307 }
308
309 pub fn collider(&self) -> ColliderId {
310 self.collider
311 }
312
313 pub fn feet(&self) -> [f32; 3] {
314 self.feet_vec().to_array()
315 }
316
317 pub fn center(&self) -> [f32; 3] {
318 self.center.to_array()
319 }
320
321 pub fn velocity(&self) -> [f32; 3] {
322 self.velocity.to_array()
323 }
324
325 pub fn up(&self) -> [f32; 3] {
326 self.up.to_array()
327 }
328
329 pub fn grounded(&self) -> bool {
330 self.grounded
331 }
332
333 pub fn sliding(&self) -> bool {
334 self.sliding
335 }
336
337 pub fn on_wall(&self) -> Option<[f32; 3]> {
338 self.wall
339 .filter(|_| self.since_wall == 0)
340 .map(|normal| normal.to_array())
341 }
342
343 pub fn ground(&self) -> Option<BodyId> {
344 self.ground
345 }
346
347 pub fn ticks(&self) -> u64 {
348 self.ticks
349 }
350
351 pub fn teleport(&mut self, world: &mut World, feet: [f32; 3]) {
352 let mut center = vec(feet) + self.up * (self.desc.height * 0.5);
353 if let Some(plane) = self.desc.plane {
354 center[plane.axis()] = self.anchor;
355 }
356 self.center = center;
357 self.velocity = Vec3::ZERO;
358 self.ground = None;
359 self.grounded = false;
360 world.set_pose(self.body, self.pose());
361 }
362
363 pub fn remove(self, world: &mut World) -> bool {
364 world.remove_body(self.body)
365 }
366
367 pub fn step(&mut self, world: &mut World) {
368 let dt = world.dt();
369 let desc = self.desc;
370 let gravity = vec(world.gravity_at(self.center.to_array())) * desc.gravity_scale;
371 self.up = level_up(up_from(gravity, self.up), desc.plane, self.up);
372 let up = self.up;
373 let pull = gravity.length();
374
375 let pressed = self.intent.jump && !self.held;
376 self.held = self.intent.jump;
377 self.buffered = if pressed {
378 Some(0)
379 } else {
380 self.buffered
381 .map(|age| age.saturating_add(1))
382 .filter(|age| *age <= desc.jump_buffer)
383 };
384
385 let wish = {
386 let raw = flatten(vec(self.intent.direction), desc.plane);
387 let level = raw - up * raw.dot(up);
388 let length = level.length();
389 if length > 1.0 { level / length } else { level }
390 } * desc.speed;
391
392 let mut rise = self.velocity.dot(up);
393 let mut level = self.velocity - up * rise;
394 let rate = if self.grounded {
395 desc.acceleration
396 } else if self.sliding {
397 0.0
398 } else {
399 desc.air_acceleration
400 };
401 level = toward(level, wish, rate * dt);
402
403 if self.grounded && rise < 0.0 {
404 rise = 0.0;
405 }
406 let can_jump = self.grounded || (self.since_ground <= desc.coyote && !self.jumped);
407 let mut jumped_now = false;
408 if self.buffered.is_some() {
409 if can_jump {
410 rise = desc.jump_speed;
411 jumped_now = true;
412 } else if let (Some([away, upward]), Some(normal)) = (desc.wall_jump, self.wall)
413 && self.since_wall <= desc.coyote
414 {
415 let off = flatten(normal - up * normal.dot(up), desc.plane).normalize_or_zero();
416 level = off * away;
417 rise = upward;
418 jumped_now = true;
419 }
420 }
421 if jumped_now {
422 self.buffered = None;
423 self.jumped = true;
424 self.rising = true;
425 self.grounded = false;
426 }
427 if self.rising && !self.intent.jump && rise > 0.0 {
428 rise *= 1.0 - desc.variable_jump;
429 self.rising = false;
430 }
431 rise -= pull * dt;
432 if rise <= 0.0 {
433 self.rising = false;
434 }
435 if let Some(slide) = desc.wall_slide
436 && !self.grounded
437 && self.wall.is_some()
438 && self.since_wall == 0
439 && rise < -slide
440 {
441 rise = -slide;
442 }
443 rise = rise.max(-desc.max_fall);
444
445 let carry = if desc.ride {
446 self.carry(world, dt)
447 } else {
448 Vec3::ZERO
449 };
450 let desired = flatten((level + up * rise) * dt, desc.plane);
451
452 let controller = desc.controller(up, rise > 0.0);
453 let shape = SharedShape::capsule_y(desc.half_height(), desc.radius);
454 let start = self.iso();
455 let mut collisions: Vec<CharacterCollision> = Vec::new();
456 let unseen = RigidBodySet::new();
457 let physics = &world.physics;
458 let (freed, moved) = {
459 let pipeline = physics.broad_phase.as_query_pipeline(
460 physics.narrow_phase.query_dispatcher(),
461 &unseen,
462 &physics.colliders,
463 self.filter(),
464 );
465 let freeing = KinematicCharacterController {
466 snap_to_ground: None,
467 autostep: None,
468 ..controller
469 };
470 let freed = flatten(
471 freeing
472 .move_shape(dt, &pipeline, shape.as_ref(), &start, Vec3::ZERO, |_| {})
473 .translation,
474 desc.plane,
475 );
476 let start = Iso::from_parts(start.translation + freed, start.rotation);
477 let moved = controller.move_shape(
478 dt,
479 &pipeline,
480 shape.as_ref(),
481 &start,
482 desired,
483 |collision| collisions.push(collision),
484 );
485 (freed, moved)
486 };
487 let translation = freed + flatten(moved.translation, desc.plane);
488 let mut center = self.center + translation;
489 if carry != Vec3::ZERO {
490 let along = carry.normalize_or_zero();
491 let at = Iso::from_parts(center, turn(up));
492 let reach = self
493 .cast(world, &shape, &at, along, carry.length(), self.ground)
494 .map_or(carry.length(), |(_, hit)| hit.time_of_impact);
495 center += flatten(along * reach, desc.plane);
496 }
497 if let Some(plane) = desc.plane {
498 center[plane.axis()] = self.anchor;
499 }
500
501 if desc.push && desc.mass > 0.0 && !collisions.is_empty() {
502 let physics = &mut world.physics;
503 let mut pipeline = physics.broad_phase.as_query_pipeline_mut(
504 physics.narrow_phase.query_dispatcher(),
505 &mut physics.bodies,
506 &mut physics.colliders,
507 self.filter(),
508 );
509 controller.solve_character_collision_impulses(
510 dt,
511 &mut pipeline,
512 shape.as_ref(),
513 desc.mass,
514 &collisions,
515 );
516 }
517
518 let below = self.cast(
519 world,
520 &shape,
521 &Iso::from_parts(center, turn(up)),
522 -up,
523 desc.skin + desc.snap.max(REACH),
524 None,
525 );
526 let level_ground = cosf(desc.max_climb * std::f32::consts::PI / 180.0);
527 let floor = self
528 .floor_normal(world, center, level_ground)
529 .or(below.map(|(_, hit)| hit.normal1));
530 let steep = floor.is_some_and(|normal| normal.dot(up) < level_ground - 1.0e-4);
531 self.grounded = moved.grounded && !steep && !jumped_now && rise <= 0.0;
532 self.sliding = moved.grounded && steep && !jumped_now;
533 if self.sliding {
534 let along = translation / dt;
535 rise = along.dot(up).min(rise.max(0.0));
536 level = along - up * along.dot(up);
537 } else if self.grounded {
538 rise = 0.0;
539 } else if rise > 0.0
540 && translation.dot(up) < rise * dt * 0.5
541 && collisions.iter().any(|c| c.hit.normal1.dot(up) < -WALL)
542 {
543 rise = (translation.dot(up) / dt).max(0.0);
544 self.rising = false;
545 }
546 for collision in &collisions {
547 let normal = collision.hit.normal1;
548 if normal.dot(up).abs() >= WALL {
549 continue;
550 }
551 let side = flatten(normal - up * normal.dot(up), desc.plane).normalize_or_zero();
552 let into = level.dot(side);
553 if into < 0.0 {
554 level -= side * into;
555 }
556 }
557 self.velocity = level + up * rise;
558
559 let wall = collisions
560 .iter()
561 .map(|collision| collision.hit.normal1)
562 .find(|normal| normal.dot(up).abs() < WALL)
563 .or_else(|| {
564 if self.grounded || wish == Vec3::ZERO {
565 return None;
566 }
567 let at = Iso::from_parts(center, turn(up));
568 self.cast(
569 world,
570 &shape,
571 &at,
572 wish.normalize_or_zero(),
573 desc.skin + REACH,
574 None,
575 )
576 .map(|(_, hit)| hit.normal1)
577 .filter(|normal| normal.dot(up).abs() < WALL)
578 });
579 match wall {
580 Some(normal) if !self.grounded => {
581 self.wall = Some(normal);
582 self.since_wall = 0;
583 }
584 _ => self.since_wall = self.since_wall.saturating_add(1),
585 }
586 if self.grounded {
587 self.since_ground = 0;
588 self.jumped = false;
589 self.wall = None;
590 } else {
591 self.since_ground = self.since_ground.saturating_add(1);
592 }
593
594 self.center = center;
595 self.ground = match below {
596 Some((handle, _)) if self.grounded || self.sliding => {
597 let physics = &world.physics;
598 physics
599 .colliders
600 .get(handle)
601 .and_then(|collider| collider.parent())
602 .filter(|parent| physics.bodies.get(*parent).is_some_and(|rb| !rb.is_fixed()))
603 .map(body_id)
604 }
605 _ => None,
606 };
607 world.move_to(self.body, self.pose());
608 self.ticks += 1;
609 }
610
611 fn feet_vec(&self) -> Vec3 {
612 self.center - self.up * (self.desc.height * 0.5)
613 }
614
615 fn pose(&self) -> Pose {
616 Pose::new(self.center.to_array(), turn(self.up).to_array())
617 }
618
619 fn iso(&self) -> Iso {
620 Iso::from_parts(self.center, turn(self.up))
621 }
622
623 fn filter(&self) -> QueryFilter<'static> {
624 QueryFilter::new()
625 .exclude_sensors()
626 .exclude_rigid_body(body_handle(self.body))
627 .groups(self.desc.layers.groups())
628 }
629
630 fn carry(&self, world: &World, dt: f32) -> Vec3 {
631 let Some(ground) = self.ground else {
632 return Vec3::ZERO;
633 };
634 let feet = self.feet_vec();
635 if let Some(to) = world.planned(ground) {
636 let Some(from) = world.physics.bodies.get(body_handle(ground)) else {
637 return Vec3::ZERO;
638 };
639 let from = *from.position();
640 return to.transform_point(from.inverse_transform_point(feet)) - feet;
641 }
642 match world.physics.bodies.get(body_handle(ground)) {
643 Some(rb) if rb.is_dynamic() => rb.velocity_at_point(feet) * dt,
644 _ => Vec3::ZERO,
645 }
646 }
647
648 fn floor_normal(&self, world: &World, center: Vec3, level: f32) -> Option<Vec3> {
649 let physics = &world.physics;
650 let pipeline = physics.broad_phase.as_query_pipeline(
651 physics.narrow_phase.query_dispatcher(),
652 &physics.bodies,
653 &physics.colliders,
654 self.filter(),
655 );
656 let reach = self.desc.half_height()
657 + self.desc.radius / level.max(0.05)
658 + self.desc.skin
659 + self.desc.snap.max(REACH);
660 pipeline
661 .cast_ray_and_get_normal(&Ray::new(center, -self.up), reach, true)
662 .map(|(_, hit)| hit.normal)
663 }
664
665 fn cast(
666 &self,
667 world: &World,
668 shape: &SharedShape,
669 at: &Iso,
670 along: Vec3,
671 reach: f32,
672 ignore: Option<BodyId>,
673 ) -> Option<(ColliderHandle, ShapeCastHit)> {
674 let physics = &world.physics;
675 let skip = ignore.map(body_handle);
676 let other =
677 |_: ColliderHandle, collider: &Collider| skip.is_none() || collider.parent() != skip;
678 let pipeline = physics.broad_phase.as_query_pipeline(
679 physics.narrow_phase.query_dispatcher(),
680 &physics.bodies,
681 &physics.colliders,
682 self.filter().predicate(&other),
683 );
684 pipeline.cast_shape(
685 at,
686 along,
687 shape.as_ref(),
688 ShapeCastOptions {
689 target_distance: self.desc.skin,
690 stop_at_penetration: false,
691 max_time_of_impact: reach,
692 compute_impact_geometry_on_penetration: true,
693 },
694 )
695 }
696}
697
698fn up_from(gravity: Vec3, last: Vec3) -> Vec3 {
699 let length = gravity.length();
700 if length > 1.0e-6 {
701 -gravity / length
702 } else if last == Vec3::ZERO {
703 Vec3::Y
704 } else {
705 last
706 }
707}
708
709fn up_of(world: &World, at: Vec3, plane: Option<Plane>) -> Vec3 {
710 level_up(
711 up_from(vec(world.gravity_at(at.to_array())), Vec3::Y),
712 plane,
713 Vec3::Y,
714 )
715}
716
717fn level_up(up: Vec3, plane: Option<Plane>, last: Vec3) -> Vec3 {
718 let flat = flatten(up, plane).normalize_or_zero();
719 if flat == Vec3::ZERO { last } else { flat }
720}
721
722fn flatten(v: Vec3, plane: Option<Plane>) -> Vec3 {
723 let Some(plane) = plane else {
724 return v;
725 };
726 let mut v = v;
727 v[plane.axis()] = 0.0;
728 v
729}
730
731fn turn(up: Vec3) -> Quat {
732 let up = up.normalize_or_zero();
733 if up == Vec3::ZERO || up == Vec3::Y {
734 return Quat::IDENTITY;
735 }
736 Quat::from_rotation_arc(Vec3::Y, up)
737}
738
739fn toward(from: Vec3, to: Vec3, step: f32) -> Vec3 {
740 let gap = to - from;
741 let length = gap.length();
742 if length <= step || length <= f32::EPSILON {
743 to
744 } else {
745 from + gap / length * step
746 }
747}