1use crate::util::{py_float, Json};
40use cumulus3d_core::io::PointCloud;
41use std::cmp::Reverse;
42use std::collections::{BinaryHeap, HashMap};
43use std::hash::{BuildHasherDefault, Hasher};
44use std::path::{Path, PathBuf};
45
46pub const POSITION_EPS: f32 = 1e-4;
48
49pub const FORMAT_VERSION: i64 = 4;
51
52pub const ADD_BYTES: usize = 20;
54pub const MOVE_BYTES: usize = 20;
56pub const REMOVE_BYTES: usize = 4;
58
59#[derive(Clone, Copy, Debug, PartialEq)]
61pub struct LiveStream {
62 pub enu: bool,
64 pub origin_gps: Option<[f64; 3]>,
66}
67
68#[derive(Default)]
70pub struct KeyHasher(u64);
71
72impl Hasher for KeyHasher {
73 fn finish(&self) -> u64 {
74 self.0 ^ (self.0 >> 32)
75 }
76 fn write(&mut self, bytes: &[u8]) {
77 for &b in bytes {
78 self.0 = (self.0.rotate_left(8) ^ b as u64).wrapping_mul(0x9E37_79B9_7F4A_7C15);
79 }
80 }
81 fn write_u64(&mut self, v: u64) {
82 self.0 = v.wrapping_mul(0x9E37_79B9_7F4A_7C15);
83 }
84}
85
86#[derive(Clone, Copy, Debug)]
87struct Shown {
88 id: u32,
89 pos: [f32; 3],
90 color: [u8; 3],
91 zone: u8,
92 seen: u32,
94}
95
96#[derive(Clone, Copy, Debug, PartialEq, Eq)]
98pub struct EventStats {
99 pub n_add: usize,
101 pub n_move: usize,
103 pub n_remove: usize,
105 pub points: usize,
107 pub bytes: usize,
109}
110
111pub struct DiffStream {
113 dir: PathBuf,
114 cfg: LiveStream,
115 last: HashMap<u64, Shown, BuildHasherDefault<KeyHasher>>,
117 free: BinaryHeap<Reverse<u32>>,
119 capacity: u32,
121 events: Vec<Json>,
123 seq: i64,
124 epoch: u32,
126}
127
128impl DiffStream {
129 pub fn new(dir: PathBuf, cfg: LiveStream) -> Self {
131 Self { dir, cfg, last: HashMap::default(), free: BinaryHeap::new(), capacity: 0, events: Vec::new(), seq: 0, epoch: 0 }
132 }
133
134 pub fn events_written(&self) -> i64 {
136 self.seq
137 }
138
139 pub fn dir(&self) -> &Path {
141 &self.dir
142 }
143
144 pub fn write_event(&mut self, t: f64, zone: usize, kind: &str, cloud: &PointCloud, zones: &[usize]) -> Result<EventStats, String> {
148 let n = cloud.len();
149 if n > 0 && cloud.keys.len() != n {
150 return Err("점 키가 없는 점군은 스트림으로 낼 수 없음".into());
151 }
152 if zones.len() != n {
153 return Err("점별 구역 길이가 점 수와 다름".into());
154 }
155 self.epoch += 1;
156 let epoch = self.epoch;
157 if self.last.is_empty() {
158 self.last.reserve(n);
159 }
160 let mut adds: Vec<(u64, [f32; 3], [u8; 3], u8)> = Vec::new();
162 let mut moves: Vec<(u64, u32, [f32; 3], [u8; 3])> = Vec::new();
163 let mut gone: Vec<u32> = Vec::new();
164 let mut replaced = 0usize;
165 for (i, &p) in cloud.positions.iter().enumerate() {
166 if !p.iter().all(|v| v.is_finite()) {
167 continue;
168 }
169 let key = cloud.keys[i];
170 let color = cloud.colors.get(i).copied().unwrap_or([255; 3]);
171 let zn = zones[i].min(255) as u8;
172 match self.last.get_mut(&key) {
173 Some(s) if s.seen == epoch => {}
174 Some(s) => {
175 s.seen = epoch;
176 if s.zone != zn {
177 gone.push(s.id);
178 replaced += 1;
179 adds.push((key, p, color, zn));
180 } else if s.color != color || (0..3).any(|a| (s.pos[a] - p[a]).abs() > POSITION_EPS) {
181 moves.push((key, s.id, p, color));
182 }
183 }
184 None => adds.push((key, p, color, zn)),
185 }
186 }
187 let mut removed_keys: Vec<u64> = Vec::new();
188 for (k, s) in &self.last {
189 if s.seen != epoch {
190 removed_keys.push(*k);
191 gone.push(s.id);
192 }
193 }
194 let (cap0, mut taken) = (self.capacity, Vec::new());
196 let mut add_ids: Vec<u32> = Vec::with_capacity(adds.len());
197 for _ in 0..adds.len() {
198 let id = match self.free.pop() {
199 Some(Reverse(id)) => {
200 taken.push(id);
201 id
202 }
203 None => {
204 self.capacity += 1;
205 self.capacity - 1
206 }
207 };
208 add_ids.push(id);
209 }
210 let n_remove = gone.len();
211 let bytes = adds.len() * ADD_BYTES + moves.len() * MOVE_BYTES + n_remove * REMOVE_BYTES;
212 let mut pack: Vec<u8> = Vec::with_capacity(bytes);
213 for ((_, p, c, z), id) in adds.iter().zip(&add_ids) {
214 pack.extend_from_slice(&id.to_le_bytes());
215 for v in p {
216 pack.extend_from_slice(&v.to_le_bytes());
217 }
218 pack.extend_from_slice(c);
219 pack.push(*z);
220 }
221 for (_, id, p, c) in &moves {
222 pack.extend_from_slice(&id.to_le_bytes());
223 for v in p {
224 pack.extend_from_slice(&v.to_le_bytes());
225 }
226 pack.extend_from_slice(c);
227 pack.push(0);
228 }
229 for id in &gone {
230 pack.extend_from_slice(&id.to_le_bytes());
231 }
232 let seq = self.seq + 1;
233 let (n_add, n_move) = (adds.len(), moves.len());
234 let points = self.last.len() - removed_keys.len() + n_add - replaced;
235 let pack_name = (bytes > 0).then(|| format!("packs/e_{seq:04}.bin"));
236 let ev = Json::obj(vec![
237 ("seq", Json::Int(seq)),
238 ("t", Json::f(t, 2)),
239 ("zone", Json::Int(zone as i64)),
240 ("kind", Json::Str(kind.to_string())),
241 ("points", Json::Int(points as i64)),
242 ("pack", pack_name.clone().map_or(Json::Null, Json::Str)),
243 ("n_add", Json::Int(n_add as i64)),
244 ("n_move", Json::Int(n_move as i64)),
245 ("n_remove", Json::Int(n_remove as i64)),
246 ("bytes", Json::Int(bytes as i64)),
247 ]);
248 let written = match &pack_name {
249 Some(name) => atomic_write(&self.dir.join(name), &pack),
250 None => Ok(()),
251 };
252 self.events.push(ev);
253 let written = written.and_then(|_| self.write_manifest());
254 if let Err(e) = written {
255 self.events.pop();
256 self.capacity = cap0;
257 for id in taken {
258 self.free.push(Reverse(id));
259 }
260 return Err(e);
261 }
262 for k in &removed_keys {
264 self.last.remove(k);
265 }
266 for (k, id, p, c) in &moves {
267 if let Some(s) = self.last.get_mut(k) {
268 s.pos = *p;
269 s.color = *c;
270 debug_assert_eq!(s.id, *id);
271 }
272 }
273 self.last.reserve(adds.len().saturating_sub(replaced));
274 for ((key, pos, color, zone), id) in adds.into_iter().zip(add_ids) {
275 self.last.insert(key, Shown { id, pos, color, zone, seen: epoch });
276 }
277 self.free.extend(gone.into_iter().map(Reverse));
278 self.seq = seq;
279 Ok(EventStats { n_add, n_move, n_remove, points: self.last.len(), bytes })
280 }
281
282 fn write_manifest(&self) -> Result<(), String> {
283 let origin = match self.cfg.origin_gps {
284 Some(o) => Json::Arr(o.iter().map(|v| Json::Num(py_float(*v, 9))).collect()),
285 None => Json::Null,
286 };
287 let m = Json::obj(vec![
288 ("version", Json::Int(FORMAT_VERSION)),
289 ("frame", Json::Str(if self.cfg.enu { "enu" } else { "display" }.into())),
290 ("origin_gps", origin),
291 ("capacity", Json::Int(self.capacity as i64)),
292 ("events", Json::Arr(self.events.clone())),
293 ]);
294 atomic_write(&self.dir.join("manifest.json"), m.dump().as_bytes())
295 }
296}
297
298#[derive(Clone, Debug, Default)]
300pub struct Heartbeat {
301 pub dir: PathBuf,
303 pub stage: &'static str,
305 pub zone: usize,
307 pub positions: usize,
309 pub positions_total: Option<usize>,
311 pub images: usize,
313}
314
315impl Heartbeat {
316 pub fn write(&self, t: f64) -> Result<(), String> {
318 let total = self.positions_total.map_or(Json::Null, |n| Json::Int(n as i64));
319 let m = Json::obj(vec![
320 ("t", Json::f(t, 1)),
321 ("stage", Json::Str(self.stage.into())),
322 ("zone", Json::Int(self.zone as i64)),
323 ("positions", Json::Int(self.positions as i64)),
324 ("positions_total", total),
325 ("images", Json::Int(self.images as i64)),
326 ]);
327 atomic_write(&self.dir.join("status.json"), m.dump().as_bytes())
328 }
329}
330
331pub(crate) fn atomic_write(path: &Path, bytes: &[u8]) -> Result<(), String> {
333 if let Some(d) = path.parent() {
334 std::fs::create_dir_all(d).map_err(|e| format!("{}: {e}", d.display()))?;
335 }
336 let mut tmp = path.as_os_str().to_owned();
337 tmp.push(".tmp");
338 let tmp = PathBuf::from(tmp);
339 std::fs::write(&tmp, bytes).map_err(|e| format!("{}: {e}", tmp.display()))?;
340 std::fs::rename(&tmp, path).map_err(|e| format!("{}: {e}", path.display()))
341}
342
343#[cfg(test)]
344mod tests {
345 use super::*;
346
347 fn tmpdir(name: &str) -> PathBuf {
348 let d = std::env::temp_dir().join(format!("cumulus3d_diff_{}_{name}", std::process::id()));
349 let _ = std::fs::remove_dir_all(&d);
350 d
351 }
352
353 fn cfg() -> LiveStream {
354 LiveStream { enu: true, origin_gps: Some([37.5, 127.0, 30.0]) }
355 }
356
357 fn lattice() -> (PointCloud, Vec<usize>) {
359 let mut c = PointCloud::default();
360 let mut zones = Vec::new();
361 for i in 0..12 {
362 for j in 0..12 {
363 for k in 0..2 {
364 c.positions.push([i as f32 * 0.7 - 3.0, j as f32 * 0.7 - 3.0, k as f32 * 0.7]);
365 c.colors.push([(i * 20) as u8, (j * 20) as u8, 7]);
366 c.keys.push(c.positions.len() as u64);
367 zones.push((i / 6) as usize);
368 }
369 }
370 }
371 (c, zones)
372 }
373
374 fn pick(c: &PointCloud, z: &[usize], idx: &[usize]) -> (PointCloud, Vec<usize>) {
376 let mut o = PointCloud::default();
377 let mut oz = Vec::new();
378 for &i in idx {
379 o.positions.push(c.positions[i]);
380 o.colors.push(c.colors[i]);
381 o.keys.push(c.keys[i]);
382 oz.push(z[i]);
383 }
384 (o, oz)
385 }
386
387 type Ids = (Vec<u32>, Vec<u32>);
388 type Slot = Option<([f32; 3], [u8; 3], u8)>;
389
390 struct Ev {
392 pack: Option<String>,
393 n: [usize; 3],
394 bytes: usize,
395 points: usize,
396 }
397
398 fn manifest(dir: &Path) -> (Json, Vec<Ev>) {
399 let m = Json::parse(&std::fs::read_to_string(dir.join("manifest.json")).unwrap()).unwrap();
400 let Some(Json::Arr(events)) = m.get("events") else { panic!("no events") };
401 let num = |e: &Json, k: &str| e.get(k).unwrap().as_f64().unwrap() as usize;
402 let evs = events
403 .iter()
404 .map(|e| Ev {
405 pack: match e.get("pack") {
406 Some(Json::Str(p)) => Some(p.clone()),
407 _ => None,
408 },
409 n: [num(e, "n_add"), num(e, "n_move"), num(e, "n_remove")],
410 bytes: num(e, "bytes"),
411 points: num(e, "points"),
412 })
413 .collect();
414 (m, evs)
415 }
416
417 fn apply(dir: &Path, slots: &mut Vec<Slot>, ev: &Ev) -> (Vec<u32>, Vec<u32>) {
419 let Some(p) = &ev.pack else { return (vec![], vec![]) };
420 let b = std::fs::read(dir.join(p)).unwrap();
421 assert_eq!(b.len(), ev.bytes);
422 assert_eq!(b.len(), ev.n[0] * ADD_BYTES + ev.n[1] * MOVE_BYTES + ev.n[2] * REMOVE_BYTES);
423 let u = |o: usize| u32::from_le_bytes(b[o..o + 4].try_into().unwrap());
424 let f = |o: usize| f32::from_le_bytes(b[o..o + 4].try_into().unwrap());
425 let (mut added, mut removed) = (vec![], vec![]);
426 let mut o = 0;
427 let rm0 = ev.n[0] * ADD_BYTES + ev.n[1] * MOVE_BYTES;
429 for r in 0..ev.n[2] {
430 let id = u(rm0 + r * 4);
431 removed.push(id);
432 slots[id as usize] = None;
433 }
434 for _ in 0..ev.n[0] {
435 let id = u(o) as usize;
436 if slots.len() <= id {
437 slots.resize(id + 1, None);
438 }
439 assert!(slots[id].is_none(), "add into a live slot {id}");
440 slots[id] = Some(([f(o + 4), f(o + 8), f(o + 12)], [b[o + 16], b[o + 17], b[o + 18]], b[o + 19]));
441 added.push(id as u32);
442 o += ADD_BYTES;
443 }
444 for _ in 0..ev.n[1] {
445 let id = u(o) as usize;
446 let s = slots[id].as_mut().expect("move of an empty slot");
447 s.0 = [f(o + 4), f(o + 8), f(o + 12)];
448 s.1 = [b[o + 16], b[o + 17], b[o + 18]];
449 assert_eq!(b[o + 19], 0);
450 o += MOVE_BYTES;
451 }
452 (added, removed)
453 }
454
455 fn replay(dir: &Path) -> (Vec<Slot>, Vec<Ids>) {
456 let (_, evs) = manifest(dir);
457 let mut slots = Vec::new();
458 let ids = evs.iter().map(|e| apply(dir, &mut slots, e)).collect();
459 (slots, ids)
460 }
461
462 fn shown(slots: &[Slot]) -> Vec<([u32; 3], [u8; 3], u8)> {
463 let mut v: Vec<_> = slots.iter().flatten().map(|s| (s.0.map(f32::to_bits), s.1, s.2)).collect();
464 v.sort();
465 v
466 }
467
468 fn expect(c: &PointCloud, z: &[usize]) -> Vec<([u32; 3], [u8; 3], u8)> {
469 let mut v: Vec<_> = (0..c.len()).map(|i| (c.positions[i].map(f32::to_bits), c.colors[i], z[i] as u8)).collect();
470 v.sort();
471 v
472 }
473
474 #[test]
475 fn packs_in_order_reproduce_the_composed_cloud() {
476 let dir = tmpdir("repro");
477 let (c, z) = lattice();
478 let mut ds = DiffStream::new(dir.clone(), cfg());
479 let st = ds.write_event(1.5, 0, "refined", &c, &z).unwrap();
480 assert_eq!((st.n_add, st.n_move, st.n_remove, st.points), (c.len(), 0, 0, c.len()));
481 let keep: Vec<usize> = (0..c.len()).filter(|i| i % 5 != 0).collect();
483 let (mut c2, mut z2) = pick(&c, &z, &keep);
484 c2.positions[3][1] += 0.5;
485 c2.positions[40][2] -= 0.25;
486 for j in 0..30u64 {
487 c2.positions.push([50.0 + j as f32, 0.0, 1.0]);
488 c2.colors.push([1, 2, 3]);
489 c2.keys.push(10_000 + j);
490 z2.push(2);
491 }
492 ds.write_event(2.5, 1, "replace", &c2, &z2).unwrap();
493 let (slots, _) = replay(&dir);
494 assert_eq!(shown(&slots), expect(&c2, &z2));
495 let (m, evs) = manifest(&dir);
496 assert_eq!(m.get("version").and_then(Json::as_f64), Some(4.0));
497 assert!(matches!(m.get("frame"), Some(Json::Str(s)) if s == "enu"));
498 assert!(matches!(m.get("origin_gps"), Some(Json::Arr(a)) if a.len() == 3));
499 assert_eq!(m.get("capacity").and_then(Json::as_f64), Some(slots.len() as f64));
500 assert_eq!(evs[0].pack.as_deref(), Some("packs/e_0001.bin"));
501 assert_eq!(evs[0].bytes, c.len() * ADD_BYTES);
502 assert_eq!(evs[1].points, c2.len());
503 assert_eq!(evs[1].n, [30, 2, c.len() - keep.len()]);
504 assert!(!dir.join("manifest.json.tmp").exists());
505 let _ = std::fs::remove_dir_all(&dir);
506 }
507
508 #[test]
509 fn identical_event_writes_no_pack() {
510 let dir = tmpdir("same");
511 let (c, z) = lattice();
512 let mut ds = DiffStream::new(dir.clone(), cfg());
513 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
514 let mut c2 = c.clone();
516 c2.positions[5][0] += 5e-5;
517 let st = ds.write_event(2.0, 0, "replace", &c2, &z).unwrap();
518 assert_eq!((st.n_add, st.n_move, st.n_remove, st.bytes), (0, 0, 0, 0));
519 let (_, evs) = manifest(&dir);
520 assert_eq!(evs.len(), 2);
521 assert!(evs[1].pack.is_none());
522 assert_eq!(evs[1].n, [0, 0, 0]);
523 assert_eq!(evs[1].points, c.len());
524 assert!(!dir.join("packs/e_0002.bin").exists());
525 let _ = std::fs::remove_dir_all(&dir);
526 }
527
528 #[test]
529 fn moving_points_only_moves() {
530 let dir = tmpdir("move");
531 let (c, z) = lattice();
532 let mut ds = DiffStream::new(dir.clone(), cfg());
533 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
534 let mut c2 = c.clone();
535 for i in [10usize, 200, 101] {
536 c2.positions[i][0] += 0.3;
537 }
538 let st = ds.write_event(2.0, 1, "replace", &c2, &z).unwrap();
539 assert_eq!((st.n_add, st.n_move, st.n_remove), (0, 3, 0));
540 assert_eq!(st.bytes, 3 * MOVE_BYTES);
541 let (slots, _) = replay(&dir);
542 assert_eq!(shown(&slots), expect(&c2, &z));
543 let _ = std::fs::remove_dir_all(&dir);
544 }
545
546 #[test]
547 fn freed_ids_are_reused_only_in_the_next_event() {
548 let dir = tmpdir("reuse");
549 let (c, z) = lattice();
550 let n = c.len();
551 let mut ds = DiffStream::new(dir.clone(), cfg());
552 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
553 let (mut c2, mut z2) = pick(&c, &z, &(10..n).collect::<Vec<_>>());
555 let new_pt = |c: &mut PointCloud, z: &mut Vec<usize>, key: u64| {
556 c.positions.push([40.0 + key as f32, 1.0, 2.0]);
557 c.colors.push([9, 9, 9]);
558 c.keys.push(key);
559 z.push(0);
560 };
561 for k in 0..10 {
562 new_pt(&mut c2, &mut z2, 5000 + k);
563 }
564 let st = ds.write_event(2.0, 0, "replace", &c2, &z2).unwrap();
565 assert_eq!((st.n_add, st.n_remove), (10, 10));
566 let (_, ids) = replay(&dir);
567 let (added2, removed2) = &ids[1];
568 assert!(added2.iter().all(|a| !removed2.contains(a)), "freed id reused in the same event");
569 assert!(added2.iter().all(|&a| a as usize >= n), "adds must take new ids");
570 let (mut c3, mut z3) = (c2.clone(), z2.clone());
572 for k in 0..10 {
573 new_pt(&mut c3, &mut z3, 6000 + k);
574 }
575 let st = ds.write_event(3.0, 0, "replace", &c3, &z3).unwrap();
576 assert_eq!((st.n_add, st.n_remove), (10, 0));
577 let (slots, ids) = replay(&dir);
578 let mut want = removed2.clone();
579 want.sort();
580 let mut got = ids[2].0.clone();
581 got.sort();
582 assert_eq!(got, want);
583 assert_eq!(ids[2].0, want, "lowest free id first");
584 assert_eq!(slots.len(), n + 10, "capacity does not grow when ids are recycled");
585 assert_eq!(shown(&slots), expect(&c3, &z3));
586 let _ = std::fs::remove_dir_all(&dir);
587 }
588
589 #[test]
590 fn colour_change_is_a_move_with_the_same_id() {
591 let dir = tmpdir("colour");
592 let (c, z) = lattice();
593 let mut ds = DiffStream::new(dir.clone(), cfg());
594 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
595 let mut c2 = c.clone();
596 c2.colors[7] = [1, 1, 1];
597 c2.colors[9] = [2, 2, 2];
598 c2.positions[9][0] += 0.2;
599 let st = ds.write_event(2.0, 0, "replace", &c2, &z).unwrap();
600 assert_eq!((st.n_add, st.n_move, st.n_remove, st.points), (0, 2, 0, c.len()));
601 assert_eq!(st.bytes, 2 * MOVE_BYTES);
602 let (slots, _) = replay(&dir);
603 assert_eq!(slots.len(), c.len());
604 assert_eq!(shown(&slots), expect(&c2, &z));
605 let _ = std::fs::remove_dir_all(&dir);
606 }
607
608 #[test]
609 fn zone_change_is_remove_plus_add_with_a_new_id() {
610 let dir = tmpdir("zone");
611 let (c, z) = lattice();
612 let mut ds = DiffStream::new(dir.clone(), cfg());
613 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
614 let mut z2 = z.clone();
615 z2[7] = 5;
616 z2[8] = 6;
617 let st = ds.write_event(2.0, 0, "replace", &c, &z2).unwrap();
618 assert_eq!((st.n_add, st.n_move, st.n_remove, st.points), (2, 0, 2, c.len()));
619 let (slots, ids) = replay(&dir);
620 assert_eq!(ids[1].1, vec![7, 8]);
621 assert!(ids[1].0.iter().all(|&a| a as usize >= c.len()));
622 assert_eq!(shown(&slots), expect(&c, &z2));
623 let _ = std::fs::remove_dir_all(&dir);
624 }
625
626 #[test]
627 fn rejects_clouds_without_keys() {
628 let dir = tmpdir("nokeys");
629 let mut c = PointCloud::default();
630 c.positions.push([0.0; 3]);
631 let mut ds = DiffStream::new(dir.clone(), cfg());
632 assert!(ds.write_event(0.0, 0, "refined", &c, &[0]).is_err());
633 assert!(!dir.join("manifest.json").exists());
634 let _ = std::fs::remove_dir_all(&dir);
635 }
636}