168 lines
7.0 KiB
Rust
168 lines
7.0 KiB
Rust
use avian3d::{math::*, prelude::*};
|
||
use bevy::prelude::*;
|
||
|
||
use super::controller_common::{CharacterController, MaxSlopeAngle};
|
||
|
||
/// Kinematic bodies do not get pushed by collisions by default,
|
||
/// so it needs to be done manually.
|
||
///
|
||
/// This system handles collision response for kinematic character controllers
|
||
/// by pushing them along their contact normals by the current penetration depth,
|
||
/// and applying velocity corrections in order to snap to slopes, slide along walls,
|
||
/// and predict collisions using speculative contacts.
|
||
#[allow(clippy::type_complexity)]
|
||
pub fn kinematic_controller_collisions(
|
||
collisions: Collisions,
|
||
bodies: Query<&RigidBody>,
|
||
collider_rbs: Query<&ColliderOf, Without<Sensor>>,
|
||
mut character_controllers: Query<
|
||
(&mut Position, &mut LinearVelocity, Option<&MaxSlopeAngle>),
|
||
(With<RigidBody>, With<CharacterController>),
|
||
>,
|
||
time: Res<Time>,
|
||
) {
|
||
// Iterate through collisions and move the kinematic body to resolve penetration
|
||
for contacts in collisions.iter() {
|
||
// Get the rigid body entities of the colliders (colliders could be children)
|
||
let Ok(
|
||
[
|
||
&ColliderOf { rigid_body: rb1 },
|
||
&ColliderOf { rigid_body: rb2 },
|
||
],
|
||
) = collider_rbs.get_many([contacts.entity1, contacts.entity2])
|
||
else {
|
||
continue;
|
||
};
|
||
|
||
// Get the body of the character controller and whether it is the first
|
||
// or second entity in the collision.
|
||
let is_first: bool;
|
||
|
||
let character_rb: RigidBody;
|
||
let is_other_dynamic: bool;
|
||
|
||
let (mut position, mut linear_velocity, max_slope_angle) =
|
||
if let Ok(character) = character_controllers.get_mut(rb1) {
|
||
is_first = true;
|
||
character_rb = *bodies.get(rb1).unwrap();
|
||
is_other_dynamic = bodies.get(rb2).is_ok_and(|rb| rb.is_dynamic());
|
||
character
|
||
} else if let Ok(character) = character_controllers.get_mut(rb2) {
|
||
is_first = false;
|
||
character_rb = *bodies.get(rb2).unwrap();
|
||
is_other_dynamic = bodies.get(rb1).is_ok_and(|rb| rb.is_dynamic());
|
||
character
|
||
} else {
|
||
continue;
|
||
};
|
||
|
||
// This system only handles collision response for kinematic character controllers.
|
||
if !character_rb.is_kinematic() {
|
||
continue;
|
||
}
|
||
|
||
// Iterate through contact manifolds and their contacts.
|
||
// Each contact in a single manifold shares the same contact normal.
|
||
for manifold in contacts.manifolds.iter() {
|
||
let normal = if is_first {
|
||
-manifold.normal
|
||
} else {
|
||
manifold.normal
|
||
};
|
||
|
||
let mut deepest_penetration: Scalar = Scalar::MIN;
|
||
|
||
// Solve each penetrating contact in the manifold.
|
||
for contact in manifold.points.iter() {
|
||
if contact.penetration > 0.0 {
|
||
position.0 += normal * contact.penetration;
|
||
}
|
||
deepest_penetration = deepest_penetration.max(contact.penetration);
|
||
}
|
||
|
||
// For now, this system only handles velocity corrections for collisions against static geometry.
|
||
if is_other_dynamic {
|
||
continue;
|
||
}
|
||
|
||
// Determine if the slope is climbable or if it's too steep to walk on.
|
||
let slope_angle = normal.angle_between(Vector::Y);
|
||
let climbable = max_slope_angle.is_some_and(|angle| slope_angle.abs() <= angle.0);
|
||
|
||
if deepest_penetration > 0.0 {
|
||
// If the slope is climbable, snap the velocity so that the character
|
||
// up and down the surface smoothly.
|
||
if climbable {
|
||
// Points in the normal's direction in the XZ plane.
|
||
|
||
let normal_direction_xz =
|
||
normal.reject_from_normalized(Vector::Y).normalize_or_zero();
|
||
|
||
// The movement speed along the direction above.
|
||
let linear_velocity_xz = linear_velocity.dot(normal_direction_xz);
|
||
|
||
// Snap the Y speed based on the speed at which the character is moving
|
||
// up or down the slope, and how steep the slope is.
|
||
//
|
||
// A 2D visualization of the slope, the contact normal, and the velocity components:
|
||
//
|
||
// ╱
|
||
// normal ╱
|
||
// * ╱
|
||
// │ * ╱ velocity_x
|
||
// │ * - - - - - -
|
||
// │ * | velocity_y
|
||
// │ * |
|
||
// *───────────────────*
|
||
|
||
let max_y_speed = -linear_velocity_xz * slope_angle.tan();
|
||
if linear_velocity.y < 0.0 {
|
||
linear_velocity.y = linear_velocity.y.max(max_y_speed);
|
||
}
|
||
} else {
|
||
// The character is intersecting an unclimbable object, like a wall.
|
||
// We want the character to slide along the surface, similarly to
|
||
// a collide-and-slide algorithm.
|
||
|
||
// Don't apply an impulse if the character is moving away from the surface.
|
||
if linear_velocity.dot(normal) > 0.0 {
|
||
continue;
|
||
}
|
||
|
||
// Slide along the surface, rejecting the velocity along the contact normal.
|
||
let impulse = linear_velocity.reject_from_normalized(normal);
|
||
linear_velocity.0 = impulse;
|
||
}
|
||
} else {
|
||
// The character is not yet intersecting the other object,
|
||
// but the narrow phase detected a speculative collision.
|
||
//
|
||
// We need to push back the part of the velocity
|
||
// that would cause penetration within the next frame.
|
||
|
||
let normal_speed = linear_velocity.dot(normal);
|
||
|
||
// Don't apply an impulse if the character is moving away from the surface.
|
||
if normal_speed > 0.0 {
|
||
continue;
|
||
}
|
||
|
||
// Compute the impulse to apply.
|
||
let impulse_magnitude =
|
||
normal_speed - (deepest_penetration / time.delta_secs_f64().adjust_precision());
|
||
let mut impulse = impulse_magnitude * normal;
|
||
|
||
// Apply the impulse differently depending on the slope angle.
|
||
if climbable {
|
||
// Avoid sliding down slopes.
|
||
linear_velocity.y -= impulse.y.min(0.0);
|
||
} else {
|
||
// Avoid climbing up walls.
|
||
impulse.y = impulse.y.max(0.0);
|
||
linear_velocity.0 -= impulse;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|