use crate::events::{Event, Meta};
use crate::post::{LiveDisplay, LiveOptions};
use cumulus3d_core::io::PointCloud;
use cumulus3d_core::Reconstruction;
use std::cmp::Reverse;
use std::collections::{BTreeMap, BTreeSet, BinaryHeap, HashMap};
use std::hash::{BuildHasherDefault, Hasher};
use std::sync::{Arc, Mutex};
use std::time::SystemTime;
pub const POSITION_EPS: f32 = 1e-4;
#[derive(Default)]
pub struct KeyHasher(u64);
impl Hasher for KeyHasher {
fn finish(&self) -> u64 {
self.0 ^ (self.0 >> 32)
}
fn write(&mut self, bytes: &[u8]) {
for &b in bytes {
self.0 = (self.0.rotate_left(8) ^ b as u64).wrapping_mul(0x9E37_79B9_7F4A_7C15);
}
}
fn write_u64(&mut self, v: u64) {
self.0 = v.wrapping_mul(0x9E37_79B9_7F4A_7C15);
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PointAdd {
pub id: u32,
pub xyz: [f32; 3],
pub rgb: [u8; 3],
pub zone: u8,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PointMove {
pub id: u32,
pub xyz: [f32; 3],
pub rgb: [u8; 3],
}
#[derive(Clone, Debug, Default, PartialEq)]
pub struct DisplayDelta {
pub seq: u64,
pub adds: Vec<PointAdd>,
pub moves: Vec<PointMove>,
pub removes: Vec<u32>,
pub total_points: usize,
pub capacity: u32,
}
impl DisplayDelta {
pub fn is_empty(&self) -> bool {
self.adds.is_empty() && self.moves.is_empty() && self.removes.is_empty()
}
}
#[derive(Clone, Copy, Debug)]
struct Shown {
id: u32,
pos: [f32; 3],
color: [u8; 3],
zone: u8,
seen: u32,
}
#[derive(Debug)]
pub struct PendingDelta {
delta: DisplayDelta,
epoch: u32,
add_keys: Vec<u64>,
move_keys: Vec<u64>,
removed_keys: Vec<u64>,
replaced: usize,
taken: Vec<u32>,
cap0: u32,
}
impl PendingDelta {
pub fn delta(&self) -> &DisplayDelta {
&self.delta
}
}
#[derive(Default)]
pub struct KeyedDiff {
last: HashMap<u64, Shown, BuildHasherDefault<KeyHasher>>,
free: BinaryHeap<Reverse<u32>>,
capacity: u32,
seq: u64,
epoch: u32,
}
impl KeyedDiff {
pub fn new() -> Self {
Self::default()
}
pub fn seq(&self) -> u64 {
self.seq
}
pub fn capacity(&self) -> u32 {
self.capacity
}
pub fn len(&self) -> usize {
self.last.len()
}
pub fn is_empty(&self) -> bool {
self.last.is_empty()
}
pub fn apply(&mut self, cloud: &PointCloud, zones: &[usize]) -> Result<DisplayDelta, String> {
let p = self.prepare(cloud, zones)?;
Ok(self.commit(p))
}
pub fn unchanged(&mut self) -> DisplayDelta {
self.seq += 1;
DisplayDelta { seq: self.seq, total_points: self.last.len(), capacity: self.capacity, ..Default::default() }
}
pub fn prepare(&mut self, cloud: &PointCloud, zones: &[usize]) -> Result<PendingDelta, String> {
let n = cloud.len();
if n > 0 && cloud.keys.len() != n {
return Err("점 키가 없는 점군은 스트림으로 낼 수 없음".into());
}
if zones.len() != n {
return Err("점별 구역 길이가 점 수와 다름".into());
}
self.epoch += 1;
let epoch = self.epoch;
if self.last.is_empty() {
self.last.reserve(n);
}
let mut adds: Vec<(u64, [f32; 3], [u8; 3], u8)> = Vec::new();
let mut moves: Vec<(u64, u32, [f32; 3], [u8; 3])> = Vec::new();
let mut gone: Vec<u32> = Vec::new();
let mut replaced = 0usize;
for (i, &p) in cloud.positions.iter().enumerate() {
if !p.iter().all(|v| v.is_finite()) {
continue;
}
let key = cloud.keys[i];
let color = cloud.colors.get(i).copied().unwrap_or([255; 3]);
let zn = zones[i].min(255) as u8;
match self.last.get_mut(&key) {
Some(s) if s.seen == epoch => {}
Some(s) => {
s.seen = epoch;
if s.zone != zn {
gone.push(s.id);
replaced += 1;
adds.push((key, p, color, zn));
} else if s.color != color || (0..3).any(|a| (s.pos[a] - p[a]).abs() > POSITION_EPS) {
moves.push((key, s.id, p, color));
}
}
None => adds.push((key, p, color, zn)),
}
}
let mut removed_keys: Vec<u64> = Vec::new();
for (k, s) in &self.last {
if s.seen != epoch {
removed_keys.push(*k);
gone.push(s.id);
}
}
let (cap0, mut taken) = (self.capacity, Vec::new());
let mut add_ids: Vec<u32> = Vec::with_capacity(adds.len());
for _ in 0..adds.len() {
let id = match self.free.pop() {
Some(Reverse(id)) => {
taken.push(id);
id
}
None => {
self.capacity += 1;
self.capacity - 1
}
};
add_ids.push(id);
}
let total_points = self.last.len() - removed_keys.len() + adds.len() - replaced;
let delta = DisplayDelta {
seq: self.seq + 1,
adds: adds.iter().zip(&add_ids).map(|((_, p, c, z), id)| PointAdd { id: *id, xyz: *p, rgb: *c, zone: *z }).collect(),
moves: moves.iter().map(|(_, id, p, c)| PointMove { id: *id, xyz: *p, rgb: *c }).collect(),
removes: gone,
total_points,
capacity: self.capacity,
};
Ok(PendingDelta {
delta,
epoch,
add_keys: adds.into_iter().map(|a| a.0).collect(),
move_keys: moves.into_iter().map(|m| m.0).collect(),
removed_keys,
replaced,
taken,
cap0,
})
}
pub fn commit(&mut self, p: PendingDelta) -> DisplayDelta {
let PendingDelta { delta, epoch, add_keys, move_keys, removed_keys, replaced, .. } = p;
for k in &removed_keys {
self.last.remove(k);
}
for (k, m) in move_keys.iter().zip(&delta.moves) {
if let Some(s) = self.last.get_mut(k) {
s.pos = m.xyz;
s.color = m.rgb;
debug_assert_eq!(s.id, m.id);
}
}
self.last.reserve(add_keys.len().saturating_sub(replaced));
for (key, a) in add_keys.into_iter().zip(&delta.adds) {
self.last.insert(key, Shown { id: a.id, pos: a.xyz, color: a.rgb, zone: a.zone, seen: epoch });
}
self.free.extend(delta.removes.iter().copied().map(Reverse));
self.seq = delta.seq;
delta
}
pub fn abort(&mut self, p: PendingDelta) {
self.capacity = p.cap0;
for id in p.taken {
self.free.push(Reverse(id));
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct MirrorPoint {
pub xyz: [f32; 3],
pub rgb: [u8; 3],
pub zone: u8,
}
#[derive(Clone, Debug, Default)]
pub struct DisplayMirror {
pub slots: Vec<Option<MirrorPoint>>,
}
impl DisplayMirror {
pub fn new() -> Self {
Self::default()
}
pub fn apply(&mut self, d: &DisplayDelta) -> Result<(), String> {
for &id in &d.removes {
match self.slots.get_mut(id as usize) {
Some(s @ Some(_)) => *s = None,
_ => return Err(format!("remove of an empty slot {id}")),
}
}
if self.slots.len() < d.capacity as usize {
self.slots.resize(d.capacity as usize, None);
}
for a in &d.adds {
let s = self.slots.get_mut(a.id as usize).ok_or_else(|| format!("add beyond capacity: {}", a.id))?;
if s.is_some() {
return Err(format!("add into a live slot {}", a.id));
}
*s = Some(MirrorPoint { xyz: a.xyz, rgb: a.rgb, zone: a.zone });
}
for m in &d.moves {
match self.slots.get_mut(m.id as usize) {
Some(Some(p)) => {
p.xyz = m.xyz;
p.rgb = m.rgb;
}
_ => return Err(format!("move of an empty slot {}", m.id)),
}
}
Ok(())
}
pub fn len(&self) -> usize {
self.slots.iter().flatten().count()
}
pub fn is_empty(&self) -> bool {
self.len() == 0
}
pub fn points(&self) -> impl Iterator<Item = (u32, &MirrorPoint)> + '_ {
self.slots.iter().enumerate().filter_map(|(i, s)| s.as_ref().map(|p| (i as u32, p)))
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
#[non_exhaustive]
pub enum DeltaKind {
Refined,
Preview,
Replace,
Final,
}
impl DeltaKind {
pub fn as_str(self) -> &'static str {
match self {
DeltaKind::Preview => "preview",
DeltaKind::Refined => "refined",
DeltaKind::Replace => "replace",
DeltaKind::Final => "final",
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct DisplayFrame {
pub enu: bool,
pub origin_gps: Option<[f64; 3]>,
}
pub struct DisplayFeed {
live: LiveDisplay,
diff: KeyedDiff,
refined: BTreeMap<usize, Arc<PointCloud>>,
refined_models: BTreeMap<usize, Arc<Reconstruction>>,
previews: BTreeSet<usize>,
quick: BTreeMap<usize, Arc<PointCloud>>,
t0: Option<SystemTime>,
origin: Arc<Mutex<Option<[f64; 3]>>>,
shown_any: bool,
}
impl DisplayFeed {
pub fn new(opts: LiveOptions) -> Self {
Self {
live: LiveDisplay::new(opts),
diff: KeyedDiff::new(),
refined: BTreeMap::new(),
refined_models: BTreeMap::new(),
previews: BTreeSet::new(),
quick: BTreeMap::new(),
t0: None,
origin: Arc::new(Mutex::new(None)),
shown_any: false,
}
}
pub fn with_origin(mut self, origin: Arc<Mutex<Option<[f64; 3]>>>) -> Self {
self.origin = origin;
self
}
pub fn set_origin(&self, origin: Option<[f64; 3]>) {
*self.origin.lock().unwrap_or_else(|p| p.into_inner()) = origin;
}
pub fn frame(&self) -> DisplayFrame {
let o = *self.origin.lock().unwrap_or_else(|p| p.into_inner());
DisplayFrame { enu: o.is_some(), origin_gps: o }
}
pub fn diff(&self) -> &KeyedDiff {
&self.diff
}
fn drop_zone(&mut self, k: usize) {
self.refined.remove(&k);
self.refined_models.remove(&k);
self.previews.remove(&k);
self.quick.remove(&k);
self.live.forget(k);
}
fn emit_delta(&mut self, meta: &Meta, zone: usize, kind: DeltaKind, t: f64) -> Option<Event> {
let clouds: BTreeMap<usize, &PointCloud> = self.refined.iter().map(|(j, c)| (*j, &**c)).collect();
let models: BTreeMap<usize, &Reconstruction> = self.refined_models.iter().map(|(j, m)| (*j, &**m)).collect();
self.live.set_relocate(!self.previews.is_empty());
let (mut display, mut zones) = self.live.compose_zoned(&clouds, &models);
let mut seen: std::collections::HashSet<u64> = display.keys.iter().copied().collect();
for (j, q) in &self.quick {
for i in 0..q.len() {
if !seen.insert(q.keys[i]) {
continue;
}
display.positions.push(q.positions[i]);
display.colors.push(q.colors.get(i).copied().unwrap_or([255, 255, 255]));
display.keys.push(q.keys[i]);
if !display.normals.is_empty() {
display.normals.push([0.0; 3]);
}
zones.push(*j);
}
}
match self.diff.apply(&display, &zones) {
Ok(delta) => {
self.shown_any = true;
Some(Event::DisplayDelta { meta: meta.clone(), delta: Arc::new(delta), zone, kind, t, frame: self.frame() })
}
Err(msg) => Some(Event::Warning { meta: meta.clone(), message: format!("display delta: {msg}") }),
}
}
pub fn handle(&mut self, e: &Event) -> Option<Event> {
let meta: &Meta = e.meta();
if matches!(e, Event::Started { .. }) || self.t0.is_none() {
self.t0 = Some(meta.at);
}
let rel = |at: SystemTime, t0: Option<SystemTime>| {
let secs = |t: SystemTime| t.duration_since(SystemTime::UNIX_EPOCH).map_or(0.0, |d| d.as_secs_f64());
secs(at) - secs(t0.unwrap_or(at))
};
match e {
Event::ZonePreview { range, cloud, dense: false, frame, .. } if frame == "display" && !cloud.is_empty() && cloud.has_keys() => {
if self.refined.contains_key(&range.zone) {
return None;
}
self.quick.insert(range.zone, Arc::clone(cloud));
self.emit_delta(meta, range.zone, DeltaKind::Preview, rel(meta.at, self.t0))
}
Event::ZonePreview { range, frame, .. } if frame != "display" => {
self.previews.insert(range.zone);
None
}
Event::ZoneRefinedPose { range, model, .. } => {
self.refined_models.insert(range.zone, Arc::clone(model));
self.live.forget(range.zone);
None
}
Event::ZoneRefined { range, cloud, dense, .. } if *dense => {
let kind = if self.refined.contains_key(&range.zone) { DeltaKind::Replace } else { DeltaKind::Refined };
self.refined.insert(range.zone, Arc::clone(cloud));
self.quick.remove(&range.zone);
self.emit_delta(meta, range.zone, kind, rel(meta.at, self.t0))
}
Event::ZoneInvalidated { zone, .. } => {
self.drop_zone(*zone);
None
}
Event::AllRefinedDone { .. } if self.shown_any => {
self.shown_any = false;
let zone = self.refined.keys().next_back().copied().unwrap_or(0);
Some(Event::DisplayDelta {
meta: meta.clone(),
delta: Arc::new(self.diff.unchanged()),
zone,
kind: DeltaKind::Final,
t: rel(meta.at, self.t0),
frame: self.frame(),
})
}
_ => None,
}
}
}
#[cfg(test)]
mod tests {
use super::*;
fn cloud(n: usize) -> (PointCloud, Vec<usize>) {
let mut c = PointCloud::default();
let mut z = Vec::new();
for i in 0..n {
c.positions.push([i as f32, 0.0, 1.0]);
c.colors.push([i as u8, 1, 2]);
c.keys.push(100 + i as u64);
z.push(i % 2);
}
(c, z)
}
fn keep(c: &PointCloud, z: &[usize], idx: &[usize]) -> (PointCloud, Vec<usize>) {
let mut o = PointCloud::default();
let mut oz = Vec::new();
for &i in idx {
o.positions.push(c.positions[i]);
o.colors.push(c.colors[i]);
o.keys.push(c.keys[i]);
oz.push(z[i]);
}
(o, oz)
}
#[test]
fn first_delta_adds_everything_with_dense_ids() {
let mut d = KeyedDiff::new();
let (c, z) = cloud(10);
let x = d.apply(&c, &z).unwrap();
assert_eq!((x.seq, x.adds.len(), x.moves.len(), x.removes.len(), x.total_points, x.capacity), (1, 10, 0, 0, 10, 10));
let ids: Vec<u32> = x.adds.iter().map(|a| a.id).collect();
assert_eq!(ids, (0..10).collect::<Vec<_>>());
assert_eq!(x.adds[3].zone, 1);
}
#[test]
fn move_on_position_or_colour_but_not_on_a_wobble() {
let mut d = KeyedDiff::new();
let (c, z) = cloud(10);
d.apply(&c, &z).unwrap();
let mut c2 = c.clone();
c2.positions[1][0] += 5e-5;
assert!(d.apply(&c2, &z).unwrap().is_empty());
c2.positions[2][1] += 0.5;
c2.colors[4] = [9, 9, 9];
let x = d.apply(&c2, &z).unwrap();
assert_eq!(x.seq, 3);
assert!(x.adds.is_empty() && x.removes.is_empty());
assert_eq!(x.moves.iter().map(|m| m.id).collect::<Vec<_>>(), vec![2, 4]);
assert_eq!(x.moves[1].rgb, [9, 9, 9]);
assert_eq!(x.total_points, 10);
}
#[test]
fn remove_frees_ids_that_are_reused_next_delta_lowest_first() {
let mut d = KeyedDiff::new();
let (c, z) = cloud(10);
d.apply(&c, &z).unwrap();
let (mut c2, mut z2) = keep(&c, &z, &[0, 1, 3, 4, 6, 7, 8, 9]);
for k in [900u64, 901] {
c2.positions.push([k as f32, 0.0, 0.0]);
c2.colors.push([1, 1, 1]);
c2.keys.push(k);
z2.push(0);
}
let x = d.apply(&c2, &z2).unwrap();
let mut rm = x.removes.clone();
rm.sort();
assert_eq!(rm, vec![2, 5]);
assert_eq!(x.adds.iter().map(|a| a.id).collect::<Vec<_>>(), vec![10, 11]);
assert_eq!((x.total_points, x.capacity), (10, 12));
let (mut c3, mut z3) = (c2.clone(), z2.clone());
for k in [902u64, 903] {
c3.positions.push([k as f32, 0.0, 0.0]);
c3.colors.push([1, 1, 1]);
c3.keys.push(k);
z3.push(0);
}
let y = d.apply(&c3, &z3).unwrap();
assert_eq!(y.adds.iter().map(|a| a.id).collect::<Vec<_>>(), vec![2, 5]);
assert_eq!(y.capacity, 12);
}
#[test]
fn zone_change_is_remove_plus_add_with_a_new_id() {
let mut d = KeyedDiff::new();
let (c, z) = cloud(6);
d.apply(&c, &z).unwrap();
let mut z2 = z.clone();
z2[1] = 7;
let x = d.apply(&c, &z2).unwrap();
assert_eq!(x.removes, vec![1]);
assert_eq!(x.adds.len(), 1);
assert_eq!((x.adds[0].id, x.adds[0].zone), (6, 7));
assert!(x.moves.is_empty());
assert_eq!(x.total_points, 6);
}
#[test]
fn errors_leave_the_state_untouched_and_abort_gives_ids_back() {
let mut d = KeyedDiff::new();
let (c, z) = cloud(4);
assert!(d.apply(&c, &z[..3]).is_err());
let mut nokeys = c.clone();
nokeys.keys.clear();
assert!(d.apply(&nokeys, &z).is_err());
assert_eq!((d.seq(), d.capacity(), d.len()), (0, 0, 0));
let p = d.prepare(&c, &z).unwrap();
assert_eq!(p.delta().adds.len(), 4);
d.abort(p);
assert_eq!((d.seq(), d.capacity(), d.len()), (0, 0, 0));
let x = d.apply(&c, &z).unwrap();
assert_eq!((x.seq, x.adds.iter().map(|a| a.id).collect::<Vec<_>>()), (1, vec![0, 1, 2, 3]));
}
#[test]
fn mirror_reproduces_the_cloud_over_a_sequence() {
let mut d = KeyedDiff::new();
let mut m = DisplayMirror::new();
let (c, z) = cloud(40);
m.apply(&d.apply(&c, &z).unwrap()).unwrap();
let (mut c2, mut z2) = keep(&c, &z, &(0..40).filter(|i| i % 3 != 0).collect::<Vec<_>>());
c2.positions[4][2] = 9.0;
z2[5] = 3;
c2.positions.push([70.0, 0.0, 0.0]);
c2.colors.push([5, 5, 5]);
c2.keys.push(7000);
z2.push(1);
m.apply(&d.apply(&c2, &z2).unwrap()).unwrap();
let mut got: Vec<_> = m.points().map(|(_, p)| (p.xyz.map(f32::to_bits), p.rgb, p.zone)).collect();
let mut want: Vec<_> = (0..c2.len()).map(|i| (c2.positions[i].map(f32::to_bits), c2.colors[i], z2[i] as u8)).collect();
got.sort();
want.sort();
assert_eq!(got, want);
assert!(m.apply(&DisplayDelta { removes: vec![999], ..Default::default() }).is_err());
}
}