1use super::World;
2use dynamis_layout::COUNTER_RESTING;
3use dynamis_layout::{
4 BodyStateRecord, COUNTER_CONSTRAINTS, COUNTER_CONTACTS, COUNTER_COUNT, COUNTER_STRIDE,
5 ConstraintRuntimeRecord, ContactRecord, Counters,
6};
7use dynamis_model::{BodyHandle, BodyState, ConstraintHandle};
8use std::collections::HashSet;
9use std::mem::size_of;
10
11pub struct ContactPoint {
12 pub position: [f32; 3],
13 pub depth: f32,
14 pub normal_impulse: f32,
15 pub tangent_impulse: f32,
16}
17
18pub struct ContactManifold {
19 pub first: BodyHandle,
20 pub second: BodyHandle,
21 pub sensor: bool,
22 pub normal: [f32; 3],
23 pub points: Vec<ContactPoint>,
24 pub step: u64,
25}
26
27const COUNTER_BYTES: u64 = COUNTER_STRIDE * COUNTER_COUNT as u64;
28
29fn pack_bytes(constraints: u32) -> u64 {
30 COUNTER_BYTES + constraints as u64 * size_of::<ConstraintRuntimeRecord>() as u64
31}
32
33fn measured_counters(bytes: &[u8]) -> Counters {
34 let stride = COUNTER_STRIDE as usize;
35 let mut counters: Counters = [0; COUNTER_COUNT];
36 for (slot, value) in counters.iter_mut().enumerate() {
37 let at = slot * stride;
38 *value = u32::from_le_bytes(bytes[at..at + 4].try_into().expect("counter slot"));
39 }
40 counters
41}
42
43impl World {
44 pub(crate) fn pack_step(&self, encoder: &mut wgpu::CommandEncoder) -> u64 {
45 let staging = self.backend.buffers.readback.pack.buffer();
46 encoder.copy_buffer_to_buffer(
47 self.backend.buffers.counters.buffer(),
48 0,
49 staging,
50 0,
51 COUNTER_BYTES,
52 );
53 let constraints = self.constraints.alive.len() as u32;
54 if constraints > 0 {
55 encoder.copy_buffer_to_buffer(
56 self.backend.buffers.constraints.runtime.buffer(),
57 0,
58 staging,
59 COUNTER_BYTES,
60 constraints as u64 * size_of::<ConstraintRuntimeRecord>() as u64,
61 );
62 }
63 pack_bytes(constraints)
64 }
65
66 pub(crate) fn consume_pack(&mut self, step: u64, bytes: &[u8]) {
67 let measured = measured_counters(bytes);
68 self.accept_measured(step, &measured);
69 let rows =
70 measured[COUNTER_CONSTRAINTS] as u64 * size_of::<ConstraintRuntimeRecord>() as u64;
71 let records =
72 dynamis_layout::decode::<ConstraintRuntimeRecord>(range(bytes, COUNTER_BYTES, rows));
73 for record in records {
74 self.accept_constraint_break(record);
75 }
76 }
77
78 pub fn poll(&mut self) {
79 self.backend.gpu.assert_alive();
80 self.collect_readbacks();
81 }
82
83 pub fn wait(&mut self) {
84 self.synchronize_states();
85 }
86
87 pub fn synchronize_states(&mut self) {
88 self.backend.gpu.assert_alive();
89 self.drain_readbacks();
90 self.backend.gpu.assert_alive();
91 if !self.bodies.states_ready {
92 self.refresh_body_states();
93 self.bodies.states_ready = true;
94 }
95 }
96
97 fn refresh_body_states(&mut self) {
98 let bytes = u64::from(self.bodies.device_count) * size_of::<BodyStateRecord>() as u64;
99 if bytes == 0 {
100 return;
101 }
102 let buffer = self.backend.buffers.bodies.states.buffer().clone();
103 let records = self.read_range(&buffer, bytes);
104 let step = self.clock.step.saturating_sub(1);
105 for record in dynamis_layout::decode::<BodyStateRecord>(&records) {
106 self.accept_body(step, record);
107 }
108 }
109
110 pub fn measured(&self) -> &Counters {
111 &self.backend.measured
112 }
113
114 pub fn contact_manifolds(&mut self) -> Vec<ContactManifold> {
115 self.wait();
116 let step = self.clock.step.saturating_sub(1);
117 let active = self.backend.measured[COUNTER_CONTACTS] as usize;
118 let capacity = (self.backend.buffers.contacts.resting.size()
119 / size_of::<ContactRecord>() as u64) as usize;
120 let resting = (self.backend.measured[COUNTER_RESTING] as usize).min(capacity);
121 let mut seen = HashSet::new();
122 let mut manifolds = Vec::with_capacity(active + resting);
123 let active_buffer = self.backend.buffers.contacts.manifolds.buffer().clone();
124 for record in self.read_manifolds(&active_buffer, active) {
125 if seen.insert((record.a, record.b)) {
126 manifolds.push(manifold_of(&record, step));
127 }
128 }
129 let resting_buffer = self.backend.buffers.contacts.resting.buffer().clone();
130 let resting_live = self.backend.buffers.contacts.resting_live.buffer().clone();
131 let live = self.read_range(&resting_live, resting as u64 * 4);
132 for (index, record) in self
133 .read_manifolds(&resting_buffer, resting)
134 .into_iter()
135 .enumerate()
136 {
137 if live[index * 4..index * 4 + 4] == [0, 0, 0, 0] {
138 continue;
139 }
140 if seen.insert((record.a, record.b)) {
141 manifolds.push(manifold_of(&record, step));
142 }
143 }
144 manifolds
145 }
146
147 fn read_manifolds(&mut self, buffer: &wgpu::Buffer, count: usize) -> Vec<ContactRecord> {
148 if count == 0 {
149 return Vec::new();
150 }
151 let bytes = (count * size_of::<ContactRecord>()) as u64;
152 let records = self.read_range(buffer, bytes);
153 dynamis_layout::decode::<ContactRecord>(&records)
154 }
155
156 pub(crate) fn read_range(&mut self, buffer: &wgpu::Buffer, bytes: u64) -> Vec<u8> {
157 if bytes == 0 {
158 return Vec::new();
159 }
160 if self
161 .backend
162 .state_readback
163 .as_ref()
164 .is_none_or(|readback| readback.size() < bytes)
165 {
166 self.backend.state_readback = Some(dynamis_gpu::BufferReadback::new(
167 self.backend.gpu.device(),
168 "dynamis state readback",
169 bytes,
170 ));
171 }
172 self.backend
173 .state_readback
174 .as_mut()
175 .expect("state readback just allocated")
176 .read(self.backend.gpu.queue(), buffer, 0, bytes)
177 }
178
179 pub(crate) fn collect_readbacks(&mut self) {
180 self.backend.gpu.poll();
181 for (step, bytes) in self.backend.buffers.readback.step.collect() {
182 self.consume_pack(step, &bytes);
183 }
184 for (_, bytes) in self.backend.buffers.readback.events.collect() {
185 self.consume_events(&bytes);
186 }
187 for (batch, bytes) in self.backend.buffers.readback.queries.collect() {
188 self.queries.pool.collect(batch, &bytes);
189 }
190 #[cfg(feature = "profile")]
191 for (_step, timings) in self.backend.pipeline.collect_timings() {
192 self.backend.pass_timings = timings;
193 }
194 }
195
196 pub(crate) fn drain_readbacks(&mut self) {
197 for (step, bytes) in self.backend.buffers.readback.step.drain() {
198 self.consume_pack(step, &bytes);
199 }
200 for (_, bytes) in self.backend.buffers.readback.events.drain() {
201 self.consume_events(&bytes);
202 }
203 for (batch, bytes) in self.backend.buffers.readback.queries.drain() {
204 self.queries.pool.collect(batch, &bytes);
205 }
206 #[cfg(feature = "profile")]
207 for (_step, timings) in self.backend.pipeline.collect_timings() {
208 self.backend.pass_timings = timings;
209 }
210 }
211
212 pub(crate) fn accept_measured(&mut self, step: u64, measured: &Counters) {
213 self.backend.measured = *measured;
214 self.backend.measured_step = Some(step);
215 self.note_events_due(step);
216 }
217
218 pub(crate) fn accept_body(&mut self, step: u64, record: BodyStateRecord) {
219 let id = record.body_id as usize;
220 assert!(
221 id < self.bodies.ids.len(),
222 "GPU readback returned an out-of-range body id"
223 );
224 if record.generation != self.bodies.ids.generation(record.body_id)
225 || self.bodies.index_of[id] == u32::MAX
226 {
227 return;
228 }
229 self.bodies.states[id] = Some(BodyState {
230 position: record.position,
231 prev_position: record.prev_position,
232 orientation: record.orientation,
233 velocity: record.velocity,
234 angular_velocity: record.angular_velocity,
235 inverse_mass: self.bodies.descriptors[id].inverse_mass,
236 com: self.bodies.descriptors[id].com,
237 sleeping: record.sleeping != 0,
238 step,
239 });
240 }
241
242 pub(crate) fn accept_constraint_break(&mut self, record: ConstraintRuntimeRecord) {
243 if record.broken == 0 {
244 return;
245 }
246 let id = record.constraint_id as usize;
247 if id >= self.constraints.ids.len() {
248 return;
249 }
250 if self.constraints.ids.generation(record.constraint_id) != record.generation
251 || self.constraints.index_of[id] == u32::MAX
252 {
253 return;
254 }
255 let handle = ConstraintHandle {
256 id: id as u32,
257 generation: record.generation,
258 };
259 self.constraints.broken.push(handle);
260 self.remove_constraint(handle);
261 }
262
263 pub fn drain_constraint_breaks(&mut self) -> Vec<ConstraintHandle> {
264 std::mem::take(&mut self.constraints.broken)
265 }
266
267 pub(crate) fn island_rounds(&self) -> u32 {
268 self.backend.reservation.bodies.max(2).ilog2() + 1
269 }
270}
271
272fn manifold_of(record: &ContactRecord, step: u64) -> ContactManifold {
273 ContactManifold {
274 first: BodyHandle {
275 id: record.first_body_id,
276 generation: record.first_generation,
277 },
278 second: BodyHandle {
279 id: record.second_body_id,
280 generation: record.second_generation,
281 },
282 sensor: record.sensor == 1,
283 normal: record.normal,
284 points: record.points[..record.point_count as usize]
285 .iter()
286 .map(|point| ContactPoint {
287 position: point.position,
288 depth: point.depth,
289 normal_impulse: point.accumulated_normal,
290 tangent_impulse: (point.accumulated_tangent_1 * point.accumulated_tangent_1
291 + point.accumulated_tangent_2 * point.accumulated_tangent_2)
292 .sqrt(),
293 })
294 .collect(),
295 step,
296 }
297}
298
299fn range(bytes: &[u8], at: u64, len: u64) -> &[u8] {
300 let at = at as usize;
301 &bytes[at..at + len as usize]
302}