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 Preview,
399 Replace,
401 Final,
403}
404
405impl DeltaKind {
406 pub fn as_str(self) -> &'static str {
408 match self {
409 DeltaKind::Preview => "preview",
410 DeltaKind::Refined => "refined",
411 DeltaKind::Replace => "replace",
412 DeltaKind::Final => "final",
413 }
414 }
415}
416
417#[derive(Clone, Copy, Debug, PartialEq)]
419pub struct DisplayFrame {
420 pub enu: bool,
422 pub origin_gps: Option<[f64; 3]>,
424}
425
426pub struct DisplayFeed {
434 live: LiveDisplay,
435 diff: KeyedDiff,
436 refined: BTreeMap<usize, Arc<PointCloud>>,
437 refined_models: BTreeMap<usize, Arc<Reconstruction>>,
438 previews: BTreeSet<usize>,
439 quick: BTreeMap<usize, Arc<PointCloud>>,
441 t0: Option<SystemTime>,
442 origin: Arc<Mutex<Option<[f64; 3]>>>,
443 shown_any: bool,
444}
445
446impl DisplayFeed {
447 pub fn new(opts: LiveOptions) -> Self {
449 Self {
450 live: LiveDisplay::new(opts),
451 diff: KeyedDiff::new(),
452 refined: BTreeMap::new(),
453 refined_models: BTreeMap::new(),
454 previews: BTreeSet::new(),
455 quick: BTreeMap::new(),
456 t0: None,
457 origin: Arc::new(Mutex::new(None)),
458 shown_any: false,
459 }
460 }
461
462 pub fn with_origin(mut self, origin: Arc<Mutex<Option<[f64; 3]>>>) -> Self {
464 self.origin = origin;
465 self
466 }
467
468 pub fn set_origin(&self, origin: Option<[f64; 3]>) {
470 *self.origin.lock().unwrap_or_else(|p| p.into_inner()) = origin;
471 }
472
473 pub fn frame(&self) -> DisplayFrame {
475 let o = *self.origin.lock().unwrap_or_else(|p| p.into_inner());
476 DisplayFrame { enu: o.is_some(), origin_gps: o }
477 }
478
479 pub fn diff(&self) -> &KeyedDiff {
481 &self.diff
482 }
483
484 fn drop_zone(&mut self, k: usize) {
485 self.refined.remove(&k);
486 self.refined_models.remove(&k);
487 self.previews.remove(&k);
488 self.quick.remove(&k);
489 self.live.forget(k);
490 }
491
492 fn emit_delta(&mut self, meta: &Meta, zone: usize, kind: DeltaKind, t: f64) -> Option<Event> {
495 let clouds: BTreeMap<usize, &PointCloud> = self.refined.iter().map(|(j, c)| (*j, &**c)).collect();
496 let models: BTreeMap<usize, &Reconstruction> = self.refined_models.iter().map(|(j, m)| (*j, &**m)).collect();
497 self.live.set_relocate(!self.previews.is_empty());
498 let (mut display, mut zones) = self.live.compose_zoned(&clouds, &models);
499 let mut seen: std::collections::HashSet<u64> = display.keys.iter().copied().collect();
500 for (j, q) in &self.quick {
501 for i in 0..q.len() {
502 if !seen.insert(q.keys[i]) {
503 continue;
504 }
505 display.positions.push(q.positions[i]);
506 display.colors.push(q.colors.get(i).copied().unwrap_or([255, 255, 255]));
507 display.keys.push(q.keys[i]);
508 if !display.normals.is_empty() {
509 display.normals.push([0.0; 3]);
510 }
511 zones.push(*j);
512 }
513 }
514 match self.diff.apply(&display, &zones) {
515 Ok(delta) => {
516 self.shown_any = true;
517 Some(Event::DisplayDelta { meta: meta.clone(), delta: Arc::new(delta), zone, kind, t, frame: self.frame() })
518 }
519 Err(msg) => Some(Event::Warning { meta: meta.clone(), message: format!("display delta: {msg}") }),
521 }
522 }
523
524 pub fn handle(&mut self, e: &Event) -> Option<Event> {
527 let meta: &Meta = e.meta();
528 if matches!(e, Event::Started { .. }) || self.t0.is_none() {
529 self.t0 = Some(meta.at);
530 }
531 let rel = |at: SystemTime, t0: Option<SystemTime>| {
532 let secs = |t: SystemTime| t.duration_since(SystemTime::UNIX_EPOCH).map_or(0.0, |d| d.as_secs_f64());
533 secs(at) - secs(t0.unwrap_or(at))
534 };
535 match e {
536 Event::ZonePreview { range, cloud, dense: false, frame, .. } if frame == "display" && !cloud.is_empty() && cloud.has_keys() => {
538 if self.refined.contains_key(&range.zone) {
539 return None;
540 }
541 self.quick.insert(range.zone, Arc::clone(cloud));
542 self.emit_delta(meta, range.zone, DeltaKind::Preview, rel(meta.at, self.t0))
543 }
544 Event::ZonePreview { range, frame, .. } if frame != "display" => {
545 self.previews.insert(range.zone);
546 None
547 }
548 Event::ZoneRefinedPose { range, model, .. } => {
549 self.refined_models.insert(range.zone, Arc::clone(model));
550 self.live.forget(range.zone);
551 None
552 }
553 Event::ZoneRefined { range, cloud, dense, .. } if *dense => {
554 let kind = if self.refined.contains_key(&range.zone) { DeltaKind::Replace } else { DeltaKind::Refined };
555 self.refined.insert(range.zone, Arc::clone(cloud));
556 self.quick.remove(&range.zone);
557 self.emit_delta(meta, range.zone, kind, rel(meta.at, self.t0))
558 }
559 Event::ZoneInvalidated { zone, .. } => {
560 self.drop_zone(*zone);
561 None
562 }
563 Event::AllRefinedDone { .. } if self.shown_any => {
564 self.shown_any = false;
565 let zone = self.refined.keys().next_back().copied().unwrap_or(0);
566 Some(Event::DisplayDelta {
567 meta: meta.clone(),
568 delta: Arc::new(self.diff.unchanged()),
569 zone,
570 kind: DeltaKind::Final,
571 t: rel(meta.at, self.t0),
572 frame: self.frame(),
573 })
574 }
575 _ => None,
576 }
577 }
578}
579
580#[cfg(test)]
581mod tests {
582 use super::*;
583
584 fn cloud(n: usize) -> (PointCloud, Vec<usize>) {
585 let mut c = PointCloud::default();
586 let mut z = Vec::new();
587 for i in 0..n {
588 c.positions.push([i as f32, 0.0, 1.0]);
589 c.colors.push([i as u8, 1, 2]);
590 c.keys.push(100 + i as u64);
591 z.push(i % 2);
592 }
593 (c, z)
594 }
595
596 fn keep(c: &PointCloud, z: &[usize], idx: &[usize]) -> (PointCloud, Vec<usize>) {
597 let mut o = PointCloud::default();
598 let mut oz = Vec::new();
599 for &i in idx {
600 o.positions.push(c.positions[i]);
601 o.colors.push(c.colors[i]);
602 o.keys.push(c.keys[i]);
603 oz.push(z[i]);
604 }
605 (o, oz)
606 }
607
608 #[test]
609 fn first_delta_adds_everything_with_dense_ids() {
610 let mut d = KeyedDiff::new();
611 let (c, z) = cloud(10);
612 let x = d.apply(&c, &z).unwrap();
613 assert_eq!((x.seq, x.adds.len(), x.moves.len(), x.removes.len(), x.total_points, x.capacity), (1, 10, 0, 0, 10, 10));
614 let ids: Vec<u32> = x.adds.iter().map(|a| a.id).collect();
615 assert_eq!(ids, (0..10).collect::<Vec<_>>());
616 assert_eq!(x.adds[3].zone, 1);
617 }
618
619 #[test]
620 fn move_on_position_or_colour_but_not_on_a_wobble() {
621 let mut d = KeyedDiff::new();
622 let (c, z) = cloud(10);
623 d.apply(&c, &z).unwrap();
624 let mut c2 = c.clone();
625 c2.positions[1][0] += 5e-5;
626 assert!(d.apply(&c2, &z).unwrap().is_empty());
627 c2.positions[2][1] += 0.5;
628 c2.colors[4] = [9, 9, 9];
629 let x = d.apply(&c2, &z).unwrap();
630 assert_eq!(x.seq, 3);
631 assert!(x.adds.is_empty() && x.removes.is_empty());
632 assert_eq!(x.moves.iter().map(|m| m.id).collect::<Vec<_>>(), vec![2, 4]);
633 assert_eq!(x.moves[1].rgb, [9, 9, 9]);
634 assert_eq!(x.total_points, 10);
635 }
636
637 #[test]
638 fn remove_frees_ids_that_are_reused_next_delta_lowest_first() {
639 let mut d = KeyedDiff::new();
640 let (c, z) = cloud(10);
641 d.apply(&c, &z).unwrap();
642 let (mut c2, mut z2) = keep(&c, &z, &[0, 1, 3, 4, 6, 7, 8, 9]);
644 for k in [900u64, 901] {
645 c2.positions.push([k as f32, 0.0, 0.0]);
646 c2.colors.push([1, 1, 1]);
647 c2.keys.push(k);
648 z2.push(0);
649 }
650 let x = d.apply(&c2, &z2).unwrap();
651 let mut rm = x.removes.clone();
652 rm.sort();
653 assert_eq!(rm, vec![2, 5]);
654 assert_eq!(x.adds.iter().map(|a| a.id).collect::<Vec<_>>(), vec![10, 11]);
655 assert_eq!((x.total_points, x.capacity), (10, 12));
656 let (mut c3, mut z3) = (c2.clone(), z2.clone());
658 for k in [902u64, 903] {
659 c3.positions.push([k as f32, 0.0, 0.0]);
660 c3.colors.push([1, 1, 1]);
661 c3.keys.push(k);
662 z3.push(0);
663 }
664 let y = d.apply(&c3, &z3).unwrap();
665 assert_eq!(y.adds.iter().map(|a| a.id).collect::<Vec<_>>(), vec![2, 5]);
666 assert_eq!(y.capacity, 12);
667 }
668
669 #[test]
670 fn zone_change_is_remove_plus_add_with_a_new_id() {
671 let mut d = KeyedDiff::new();
672 let (c, z) = cloud(6);
673 d.apply(&c, &z).unwrap();
674 let mut z2 = z.clone();
675 z2[1] = 7;
676 let x = d.apply(&c, &z2).unwrap();
677 assert_eq!(x.removes, vec![1]);
678 assert_eq!(x.adds.len(), 1);
679 assert_eq!((x.adds[0].id, x.adds[0].zone), (6, 7));
680 assert!(x.moves.is_empty());
681 assert_eq!(x.total_points, 6);
682 }
683
684 #[test]
685 fn errors_leave_the_state_untouched_and_abort_gives_ids_back() {
686 let mut d = KeyedDiff::new();
687 let (c, z) = cloud(4);
688 assert!(d.apply(&c, &z[..3]).is_err());
689 let mut nokeys = c.clone();
690 nokeys.keys.clear();
691 assert!(d.apply(&nokeys, &z).is_err());
692 assert_eq!((d.seq(), d.capacity(), d.len()), (0, 0, 0));
693 let p = d.prepare(&c, &z).unwrap();
694 assert_eq!(p.delta().adds.len(), 4);
695 d.abort(p);
696 assert_eq!((d.seq(), d.capacity(), d.len()), (0, 0, 0));
697 let x = d.apply(&c, &z).unwrap();
698 assert_eq!((x.seq, x.adds.iter().map(|a| a.id).collect::<Vec<_>>()), (1, vec![0, 1, 2, 3]));
699 }
700
701 #[test]
702 fn mirror_reproduces_the_cloud_over_a_sequence() {
703 let mut d = KeyedDiff::new();
704 let mut m = DisplayMirror::new();
705 let (c, z) = cloud(40);
706 m.apply(&d.apply(&c, &z).unwrap()).unwrap();
707 let (mut c2, mut z2) = keep(&c, &z, &(0..40).filter(|i| i % 3 != 0).collect::<Vec<_>>());
708 c2.positions[4][2] = 9.0;
709 z2[5] = 3;
710 c2.positions.push([70.0, 0.0, 0.0]);
711 c2.colors.push([5, 5, 5]);
712 c2.keys.push(7000);
713 z2.push(1);
714 m.apply(&d.apply(&c2, &z2).unwrap()).unwrap();
715 let mut got: Vec<_> = m.points().map(|(_, p)| (p.xyz.map(f32::to_bits), p.rgb, p.zone)).collect();
716 let mut want: Vec<_> = (0..c2.len()).map(|i| (c2.positions[i].map(f32::to_bits), c2.colors[i], z2[i] as u8)).collect();
717 got.sort();
718 want.sort();
719 assert_eq!(got, want);
720 assert!(m.apply(&DisplayDelta { removes: vec![999], ..Default::default() }).is_err());
721 }
722}