1use crate::events::{Event, Meta};
43use crate::post::{LiveDisplay, LiveOptions};
44use cumulus3d_core::io::PointCloud;
45use cumulus3d_core::Reconstruction;
46use std::cmp::Reverse;
47use std::collections::{BTreeMap, BTreeSet, BinaryHeap, HashMap};
48use std::hash::{BuildHasherDefault, Hasher};
49use std::sync::{Arc, Mutex};
50use std::time::SystemTime;
51
52pub const POSITION_EPS: f32 = 1e-4;
54
55#[derive(Default)]
57pub struct KeyHasher(u64);
58
59impl Hasher for KeyHasher {
60 fn finish(&self) -> u64 {
61 self.0 ^ (self.0 >> 32)
62 }
63 fn write(&mut self, bytes: &[u8]) {
64 for &b in bytes {
65 self.0 = (self.0.rotate_left(8) ^ b as u64).wrapping_mul(0x9E37_79B9_7F4A_7C15);
66 }
67 }
68 fn write_u64(&mut self, v: u64) {
69 self.0 = v.wrapping_mul(0x9E37_79B9_7F4A_7C15);
70 }
71}
72
73#[derive(Clone, Copy, Debug, PartialEq)]
75pub struct PointAdd {
76 pub id: u32,
78 pub xyz: [f32; 3],
80 pub rgb: [u8; 3],
82 pub zone: u8,
84}
85
86#[derive(Clone, Copy, Debug, PartialEq)]
88pub struct PointMove {
89 pub id: u32,
91 pub xyz: [f32; 3],
93 pub rgb: [u8; 3],
95}
96
97#[derive(Clone, Debug, Default, PartialEq)]
99pub struct DisplayDelta {
100 pub seq: u64,
102 pub adds: Vec<PointAdd>,
104 pub moves: Vec<PointMove>,
106 pub removes: Vec<u32>,
108 pub total_points: usize,
110 pub capacity: u32,
112}
113
114impl DisplayDelta {
115 pub fn is_empty(&self) -> bool {
117 self.adds.is_empty() && self.moves.is_empty() && self.removes.is_empty()
118 }
119}
120
121#[derive(Clone, Copy, Debug)]
122struct Shown {
123 id: u32,
124 pos: [f32; 3],
125 color: [u8; 3],
126 zone: u8,
127 seen: u32,
129}
130
131#[derive(Debug)]
134pub struct PendingDelta {
135 delta: DisplayDelta,
136 epoch: u32,
137 add_keys: Vec<u64>,
138 move_keys: Vec<u64>,
139 removed_keys: Vec<u64>,
140 replaced: usize,
141 taken: Vec<u32>,
142 cap0: u32,
143}
144
145impl PendingDelta {
146 pub fn delta(&self) -> &DisplayDelta {
148 &self.delta
149 }
150}
151
152#[derive(Default)]
154pub struct KeyedDiff {
155 last: HashMap<u64, Shown, BuildHasherDefault<KeyHasher>>,
156 free: BinaryHeap<Reverse<u32>>,
157 capacity: u32,
158 seq: u64,
159 epoch: u32,
160}
161
162impl KeyedDiff {
163 pub fn new() -> Self {
165 Self::default()
166 }
167
168 pub fn seq(&self) -> u64 {
170 self.seq
171 }
172
173 pub fn capacity(&self) -> u32 {
175 self.capacity
176 }
177
178 pub fn len(&self) -> usize {
180 self.last.len()
181 }
182
183 pub fn is_empty(&self) -> bool {
185 self.last.is_empty()
186 }
187
188 pub fn apply(&mut self, cloud: &PointCloud, zones: &[usize]) -> Result<DisplayDelta, String> {
191 let p = self.prepare(cloud, zones)?;
192 Ok(self.commit(p))
193 }
194
195 pub fn unchanged(&mut self) -> DisplayDelta {
197 self.seq += 1;
198 DisplayDelta { seq: self.seq, total_points: self.last.len(), capacity: self.capacity, ..Default::default() }
199 }
200
201 pub fn prepare(&mut self, cloud: &PointCloud, zones: &[usize]) -> Result<PendingDelta, String> {
204 let n = cloud.len();
205 if n > 0 && cloud.keys.len() != n {
206 return Err("점 키가 없는 점군은 스트림으로 낼 수 없음".into());
207 }
208 if zones.len() != n {
209 return Err("점별 구역 길이가 점 수와 다름".into());
210 }
211 self.epoch += 1;
212 let epoch = self.epoch;
213 if self.last.is_empty() {
214 self.last.reserve(n);
215 }
216 let mut adds: Vec<(u64, [f32; 3], [u8; 3], u8)> = Vec::new();
218 let mut moves: Vec<(u64, u32, [f32; 3], [u8; 3])> = Vec::new();
219 let mut gone: Vec<u32> = Vec::new();
220 let mut replaced = 0usize;
221 for (i, &p) in cloud.positions.iter().enumerate() {
222 if !p.iter().all(|v| v.is_finite()) {
223 continue;
224 }
225 let key = cloud.keys[i];
226 let color = cloud.colors.get(i).copied().unwrap_or([255; 3]);
227 let zn = zones[i].min(255) as u8;
228 match self.last.get_mut(&key) {
229 Some(s) if s.seen == epoch => {}
230 Some(s) => {
231 s.seen = epoch;
232 if s.zone != zn {
233 gone.push(s.id);
234 replaced += 1;
235 adds.push((key, p, color, zn));
236 } else if s.color != color || (0..3).any(|a| (s.pos[a] - p[a]).abs() > POSITION_EPS) {
237 moves.push((key, s.id, p, color));
238 }
239 }
240 None => adds.push((key, p, color, zn)),
241 }
242 }
243 let mut removed_keys: Vec<u64> = Vec::new();
244 for (k, s) in &self.last {
245 if s.seen != epoch {
246 removed_keys.push(*k);
247 gone.push(s.id);
248 }
249 }
250 let (cap0, mut taken) = (self.capacity, Vec::new());
252 let mut add_ids: Vec<u32> = Vec::with_capacity(adds.len());
253 for _ in 0..adds.len() {
254 let id = match self.free.pop() {
255 Some(Reverse(id)) => {
256 taken.push(id);
257 id
258 }
259 None => {
260 self.capacity += 1;
261 self.capacity - 1
262 }
263 };
264 add_ids.push(id);
265 }
266 let total_points = self.last.len() - removed_keys.len() + adds.len() - replaced;
267 let delta = DisplayDelta {
268 seq: self.seq + 1,
269 adds: adds.iter().zip(&add_ids).map(|((_, p, c, z), id)| PointAdd { id: *id, xyz: *p, rgb: *c, zone: *z }).collect(),
270 moves: moves.iter().map(|(_, id, p, c)| PointMove { id: *id, xyz: *p, rgb: *c }).collect(),
271 removes: gone,
272 total_points,
273 capacity: self.capacity,
274 };
275 Ok(PendingDelta {
276 delta,
277 epoch,
278 add_keys: adds.into_iter().map(|a| a.0).collect(),
279 move_keys: moves.into_iter().map(|m| m.0).collect(),
280 removed_keys,
281 replaced,
282 taken,
283 cap0,
284 })
285 }
286
287 pub fn commit(&mut self, p: PendingDelta) -> DisplayDelta {
289 let PendingDelta { delta, epoch, add_keys, move_keys, removed_keys, replaced, .. } = p;
290 for k in &removed_keys {
291 self.last.remove(k);
292 }
293 for (k, m) in move_keys.iter().zip(&delta.moves) {
294 if let Some(s) = self.last.get_mut(k) {
295 s.pos = m.xyz;
296 s.color = m.rgb;
297 debug_assert_eq!(s.id, m.id);
298 }
299 }
300 self.last.reserve(add_keys.len().saturating_sub(replaced));
301 for (key, a) in add_keys.into_iter().zip(&delta.adds) {
302 self.last.insert(key, Shown { id: a.id, pos: a.xyz, color: a.rgb, zone: a.zone, seen: epoch });
303 }
304 self.free.extend(delta.removes.iter().copied().map(Reverse));
305 self.seq = delta.seq;
306 delta
307 }
308
309 pub fn abort(&mut self, p: PendingDelta) {
311 self.capacity = p.cap0;
312 for id in p.taken {
313 self.free.push(Reverse(id));
314 }
315 }
316}
317
318#[derive(Clone, Copy, Debug, PartialEq)]
320pub struct MirrorPoint {
321 pub xyz: [f32; 3],
323 pub rgb: [u8; 3],
325 pub zone: u8,
327}
328
329#[derive(Clone, Debug, Default)]
331pub struct DisplayMirror {
332 pub slots: Vec<Option<MirrorPoint>>,
334}
335
336impl DisplayMirror {
337 pub fn new() -> Self {
339 Self::default()
340 }
341
342 pub fn apply(&mut self, d: &DisplayDelta) -> Result<(), String> {
345 for &id in &d.removes {
346 match self.slots.get_mut(id as usize) {
347 Some(s @ Some(_)) => *s = None,
348 _ => return Err(format!("remove of an empty slot {id}")),
349 }
350 }
351 if self.slots.len() < d.capacity as usize {
352 self.slots.resize(d.capacity as usize, None);
353 }
354 for a in &d.adds {
355 let s = self.slots.get_mut(a.id as usize).ok_or_else(|| format!("add beyond capacity: {}", a.id))?;
356 if s.is_some() {
357 return Err(format!("add into a live slot {}", a.id));
358 }
359 *s = Some(MirrorPoint { xyz: a.xyz, rgb: a.rgb, zone: a.zone });
360 }
361 for m in &d.moves {
362 match self.slots.get_mut(m.id as usize) {
363 Some(Some(p)) => {
364 p.xyz = m.xyz;
365 p.rgb = m.rgb;
366 }
367 _ => return Err(format!("move of an empty slot {}", m.id)),
368 }
369 }
370 Ok(())
371 }
372
373 pub fn len(&self) -> usize {
375 self.slots.iter().flatten().count()
376 }
377
378 pub fn is_empty(&self) -> bool {
380 self.len() == 0
381 }
382
383 pub fn points(&self) -> impl Iterator<Item = (u32, &MirrorPoint)> + '_ {
385 self.slots.iter().enumerate().filter_map(|(i, s)| s.as_ref().map(|p| (i as u32, p)))
386 }
387}
388
389#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
391#[non_exhaustive]
392pub enum DeltaKind {
393 Refined,
395 Replace,
397 Final,
399}
400
401impl DeltaKind {
402 pub fn as_str(self) -> &'static str {
404 match self {
405 DeltaKind::Refined => "refined",
406 DeltaKind::Replace => "replace",
407 DeltaKind::Final => "final",
408 }
409 }
410}
411
412#[derive(Clone, Copy, Debug, PartialEq)]
414pub struct DisplayFrame {
415 pub enu: bool,
417 pub origin_gps: Option<[f64; 3]>,
419}
420
421pub struct DisplayFeed {
429 live: LiveDisplay,
430 diff: KeyedDiff,
431 refined: BTreeMap<usize, Arc<PointCloud>>,
432 refined_models: BTreeMap<usize, Arc<Reconstruction>>,
433 previews: BTreeSet<usize>,
434 t0: Option<SystemTime>,
435 origin: Arc<Mutex<Option<[f64; 3]>>>,
436 shown_any: bool,
437}
438
439impl DisplayFeed {
440 pub fn new(opts: LiveOptions) -> Self {
442 Self {
443 live: LiveDisplay::new(opts),
444 diff: KeyedDiff::new(),
445 refined: BTreeMap::new(),
446 refined_models: BTreeMap::new(),
447 previews: BTreeSet::new(),
448 t0: None,
449 origin: Arc::new(Mutex::new(None)),
450 shown_any: false,
451 }
452 }
453
454 pub fn with_origin(mut self, origin: Arc<Mutex<Option<[f64; 3]>>>) -> Self {
456 self.origin = origin;
457 self
458 }
459
460 pub fn set_origin(&self, origin: Option<[f64; 3]>) {
462 *self.origin.lock().unwrap_or_else(|p| p.into_inner()) = origin;
463 }
464
465 pub fn frame(&self) -> DisplayFrame {
467 let o = *self.origin.lock().unwrap_or_else(|p| p.into_inner());
468 DisplayFrame { enu: o.is_some(), origin_gps: o }
469 }
470
471 pub fn diff(&self) -> &KeyedDiff {
473 &self.diff
474 }
475
476 fn drop_zone(&mut self, k: usize) {
477 self.refined.remove(&k);
478 self.refined_models.remove(&k);
479 self.previews.remove(&k);
480 self.live.forget(k);
481 }
482
483 pub fn handle(&mut self, e: &Event) -> Option<Event> {
486 let meta: &Meta = e.meta();
487 if matches!(e, Event::Started { .. }) || self.t0.is_none() {
488 self.t0 = Some(meta.at);
489 }
490 let rel = |at: SystemTime, t0: Option<SystemTime>| {
491 let secs = |t: SystemTime| t.duration_since(SystemTime::UNIX_EPOCH).map_or(0.0, |d| d.as_secs_f64());
492 secs(at) - secs(t0.unwrap_or(at))
493 };
494 match e {
495 Event::ZonePreview { range, .. } => {
496 self.previews.insert(range.zone);
497 None
498 }
499 Event::ZoneRefinedPose { range, model, .. } => {
500 self.refined_models.insert(range.zone, Arc::clone(model));
501 self.live.forget(range.zone);
502 None
503 }
504 Event::ZoneRefined { range, cloud, dense, .. } if *dense => {
505 let kind = if self.refined.contains_key(&range.zone) { DeltaKind::Replace } else { DeltaKind::Refined };
506 self.refined.insert(range.zone, Arc::clone(cloud));
507 let clouds: BTreeMap<usize, &PointCloud> = self.refined.iter().map(|(j, c)| (*j, &**c)).collect();
508 let models: BTreeMap<usize, &Reconstruction> = self.refined_models.iter().map(|(j, m)| (*j, &**m)).collect();
509 self.live.set_relocate(!self.previews.is_empty());
510 let (display, zones) = self.live.compose_zoned(&clouds, &models);
511 match self.diff.apply(&display, &zones) {
512 Ok(delta) => {
513 self.shown_any = true;
514 Some(Event::DisplayDelta {
515 meta: meta.clone(),
516 delta: Arc::new(delta),
517 zone: range.zone,
518 kind,
519 t: rel(meta.at, self.t0),
520 frame: self.frame(),
521 })
522 }
523 Err(msg) => Some(Event::Warning { meta: meta.clone(), message: format!("display delta: {msg}") }),
525 }
526 }
527 Event::ZoneInvalidated { zone, .. } => {
528 self.drop_zone(*zone);
529 None
530 }
531 Event::AllRefinedDone { .. } if self.shown_any => {
532 self.shown_any = false;
533 let zone = self.refined.keys().next_back().copied().unwrap_or(0);
534 Some(Event::DisplayDelta {
535 meta: meta.clone(),
536 delta: Arc::new(self.diff.unchanged()),
537 zone,
538 kind: DeltaKind::Final,
539 t: rel(meta.at, self.t0),
540 frame: self.frame(),
541 })
542 }
543 _ => None,
544 }
545 }
546}
547
548#[cfg(test)]
549mod tests {
550 use super::*;
551
552 fn cloud(n: usize) -> (PointCloud, Vec<usize>) {
553 let mut c = PointCloud::default();
554 let mut z = Vec::new();
555 for i in 0..n {
556 c.positions.push([i as f32, 0.0, 1.0]);
557 c.colors.push([i as u8, 1, 2]);
558 c.keys.push(100 + i as u64);
559 z.push(i % 2);
560 }
561 (c, z)
562 }
563
564 fn keep(c: &PointCloud, z: &[usize], idx: &[usize]) -> (PointCloud, Vec<usize>) {
565 let mut o = PointCloud::default();
566 let mut oz = Vec::new();
567 for &i in idx {
568 o.positions.push(c.positions[i]);
569 o.colors.push(c.colors[i]);
570 o.keys.push(c.keys[i]);
571 oz.push(z[i]);
572 }
573 (o, oz)
574 }
575
576 #[test]
577 fn first_delta_adds_everything_with_dense_ids() {
578 let mut d = KeyedDiff::new();
579 let (c, z) = cloud(10);
580 let x = d.apply(&c, &z).unwrap();
581 assert_eq!((x.seq, x.adds.len(), x.moves.len(), x.removes.len(), x.total_points, x.capacity), (1, 10, 0, 0, 10, 10));
582 let ids: Vec<u32> = x.adds.iter().map(|a| a.id).collect();
583 assert_eq!(ids, (0..10).collect::<Vec<_>>());
584 assert_eq!(x.adds[3].zone, 1);
585 }
586
587 #[test]
588 fn move_on_position_or_colour_but_not_on_a_wobble() {
589 let mut d = KeyedDiff::new();
590 let (c, z) = cloud(10);
591 d.apply(&c, &z).unwrap();
592 let mut c2 = c.clone();
593 c2.positions[1][0] += 5e-5;
594 assert!(d.apply(&c2, &z).unwrap().is_empty());
595 c2.positions[2][1] += 0.5;
596 c2.colors[4] = [9, 9, 9];
597 let x = d.apply(&c2, &z).unwrap();
598 assert_eq!(x.seq, 3);
599 assert!(x.adds.is_empty() && x.removes.is_empty());
600 assert_eq!(x.moves.iter().map(|m| m.id).collect::<Vec<_>>(), vec![2, 4]);
601 assert_eq!(x.moves[1].rgb, [9, 9, 9]);
602 assert_eq!(x.total_points, 10);
603 }
604
605 #[test]
606 fn remove_frees_ids_that_are_reused_next_delta_lowest_first() {
607 let mut d = KeyedDiff::new();
608 let (c, z) = cloud(10);
609 d.apply(&c, &z).unwrap();
610 let (mut c2, mut z2) = keep(&c, &z, &[0, 1, 3, 4, 6, 7, 8, 9]);
612 for k in [900u64, 901] {
613 c2.positions.push([k as f32, 0.0, 0.0]);
614 c2.colors.push([1, 1, 1]);
615 c2.keys.push(k);
616 z2.push(0);
617 }
618 let x = d.apply(&c2, &z2).unwrap();
619 let mut rm = x.removes.clone();
620 rm.sort();
621 assert_eq!(rm, vec![2, 5]);
622 assert_eq!(x.adds.iter().map(|a| a.id).collect::<Vec<_>>(), vec![10, 11]);
623 assert_eq!((x.total_points, x.capacity), (10, 12));
624 let (mut c3, mut z3) = (c2.clone(), z2.clone());
626 for k in [902u64, 903] {
627 c3.positions.push([k as f32, 0.0, 0.0]);
628 c3.colors.push([1, 1, 1]);
629 c3.keys.push(k);
630 z3.push(0);
631 }
632 let y = d.apply(&c3, &z3).unwrap();
633 assert_eq!(y.adds.iter().map(|a| a.id).collect::<Vec<_>>(), vec![2, 5]);
634 assert_eq!(y.capacity, 12);
635 }
636
637 #[test]
638 fn zone_change_is_remove_plus_add_with_a_new_id() {
639 let mut d = KeyedDiff::new();
640 let (c, z) = cloud(6);
641 d.apply(&c, &z).unwrap();
642 let mut z2 = z.clone();
643 z2[1] = 7;
644 let x = d.apply(&c, &z2).unwrap();
645 assert_eq!(x.removes, vec![1]);
646 assert_eq!(x.adds.len(), 1);
647 assert_eq!((x.adds[0].id, x.adds[0].zone), (6, 7));
648 assert!(x.moves.is_empty());
649 assert_eq!(x.total_points, 6);
650 }
651
652 #[test]
653 fn errors_leave_the_state_untouched_and_abort_gives_ids_back() {
654 let mut d = KeyedDiff::new();
655 let (c, z) = cloud(4);
656 assert!(d.apply(&c, &z[..3]).is_err());
657 let mut nokeys = c.clone();
658 nokeys.keys.clear();
659 assert!(d.apply(&nokeys, &z).is_err());
660 assert_eq!((d.seq(), d.capacity(), d.len()), (0, 0, 0));
661 let p = d.prepare(&c, &z).unwrap();
662 assert_eq!(p.delta().adds.len(), 4);
663 d.abort(p);
664 assert_eq!((d.seq(), d.capacity(), d.len()), (0, 0, 0));
665 let x = d.apply(&c, &z).unwrap();
666 assert_eq!((x.seq, x.adds.iter().map(|a| a.id).collect::<Vec<_>>()), (1, vec![0, 1, 2, 3]));
667 }
668
669 #[test]
670 fn mirror_reproduces_the_cloud_over_a_sequence() {
671 let mut d = KeyedDiff::new();
672 let mut m = DisplayMirror::new();
673 let (c, z) = cloud(40);
674 m.apply(&d.apply(&c, &z).unwrap()).unwrap();
675 let (mut c2, mut z2) = keep(&c, &z, &(0..40).filter(|i| i % 3 != 0).collect::<Vec<_>>());
676 c2.positions[4][2] = 9.0;
677 z2[5] = 3;
678 c2.positions.push([70.0, 0.0, 0.0]);
679 c2.colors.push([5, 5, 5]);
680 c2.keys.push(7000);
681 z2.push(1);
682 m.apply(&d.apply(&c2, &z2).unwrap()).unwrap();
683 let mut got: Vec<_> = m.points().map(|(_, p)| (p.xyz.map(f32::to_bits), p.rgb, p.zone)).collect();
684 let mut want: Vec<_> = (0..c2.len()).map(|i| (c2.positions[i].map(f32::to_bits), c2.colors[i], z2[i] as u8)).collect();
685 got.sort();
686 want.sort();
687 assert_eq!(got, want);
688 assert!(m.apply(&DisplayDelta { removes: vec![999], ..Default::default() }).is_err());
689 }
690}