1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
|
//! Local port of `KinematicCharacterController::solve_character_collision_impulses`.
//!
//! Rapier's version (0.35.2, still present on master) accumulates every nearby collider's
//! contact manifolds into one shared `Vec` and then slices it with `manifolds[prev_len..]`.
//! That assumes `contact_manifolds` appends, but parry's composite-shape paths (compound,
//! polyline, trimesh, ...) `mem::take` the output vec and rebuild it for the current pair
//! only. With two or more dynamic bodies inside the character's AABB the second call empties
//! the vec and the slice panics with "range start index 1 out of range for slice of length 0".
//! The character's collider here is a convex decomposition (a compound), so we hit it.
//!
//! This port uses a fresh scratch vec per collider, which is what rapier's own code intended.
use rapier2d::{
control::{CharacterCollision, CharacterLength, KinematicCharacterController},
geometry::{ContactManifold, Shape},
math::{Pose, Real},
parry::{
bounding_volume::BoundingVolume,
query::{DefaultQueryDispatcher, PersistentQueryDispatcher},
},
pipeline::QueryPipelineMut,
};
/// Apply approximate impulses to the dynamic bodies a kinematic character ran into.
pub fn solve_character_collision_impulses<'a>(
controller: &KinematicCharacterController,
dt: Real,
queries: &mut QueryPipelineMut,
character_shape: &dyn Shape,
character_mass: Real,
collisions: impl IntoIterator<Item = &'a CharacterCollision>,
) {
for collision in collisions {
solve_single_character_collision_impulse(
controller,
dt,
queries,
character_shape,
character_mass,
collision,
);
}
}
fn eval_length(length: CharacterLength, value: Real) -> Real {
match length {
CharacterLength::Relative(x) => value * x,
CharacterLength::Absolute(x) => x,
}
}
fn solve_single_character_collision_impulse(
controller: &KinematicCharacterController,
dt: Real,
queries: &mut QueryPipelineMut,
character_shape: &dyn Shape,
character_mass: Real,
collision: &CharacterCollision,
) {
let extents = character_shape.compute_local_aabb().extents();
let up_extent = extents.dot(controller.up.abs());
let movement_to_transfer =
collision.hit.normal1 * collision.translation_remaining.dot(collision.hit.normal1);
// `KinematicCharacterController::predict_ground` is private; this is its body.
let prediction = eval_length(controller.offset, up_extent) + 0.05;
let dispatcher = DefaultQueryDispatcher;
let mut manifolds: Vec<ContactManifold> = Vec::new();
// World pose of the collider each manifold was computed against: the `local_p2`
// points are in the collider's frame, which differs from its body's when offset.
let mut manifold_collider_poses: Vec<Pose> = Vec::new();
let character_aabb = character_shape
.compute_aabb(&collision.character_pos)
.loosened(prediction);
for (_, collider) in queries.as_ref().intersect_aabb_conservative(character_aabb) {
let Some(parent) = collider.parent() else {
continue;
};
let Some(body) = queries.bodies.get(parent) else {
continue;
};
if !body.is_dynamic() {
continue;
}
let pos12 = collision.character_pos.inv_mul(collider.position());
// Fresh vec per pair: parry may take/replace it rather than append.
let mut pair_manifolds: Vec<ContactManifold> = Vec::new();
let _ = dispatcher.contact_manifolds(
&pos12,
character_shape,
collider.shape(),
prediction,
&mut pair_manifolds,
&mut None,
);
for mut m in pair_manifolds {
m.data.rigid_body2 = Some(parent);
m.data.normal = collision.character_pos.rotation * m.local_n1;
manifolds.push(m);
manifold_collider_poses.push(*collider.position());
}
}
let inv_dt = if dt != 0.0 { 1.0 / dt } else { 0.0 };
let velocity_to_transfer = movement_to_transfer * inv_dt;
for (manifold, collider_pos) in manifolds.iter().zip(manifold_collider_poses.iter()) {
let Some(body_handle) = manifold.data.rigid_body2 else {
continue;
};
let Some(body) = queries.bodies.get_mut(body_handle) else {
continue;
};
for pt in &manifold.points {
if pt.dist <= prediction {
let body_mass = body.mass();
let contact_point = *collider_pos * pt.local_p2;
let delta_vel_per_contact = (velocity_to_transfer
- body.velocity_at_point(contact_point))
.dot(manifold.data.normal);
let mass_ratio = body_mass * character_mass / (body_mass + character_mass);
body.apply_impulse_at_point(
manifold.data.normal * delta_vel_per_contact.max(0.0) * mass_ratio,
contact_point,
true,
);
}
}
}
}
|
