sylva-rs 0.3.2

Terrestrial and airborne laser scanning processing for forest ecology and remote sensing research.
Documentation
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
// Sylva: LiDAR processing for forest ecology and remote sensing research.
// Copyright (C) 2026 Tim Devereux, The University of Queensland.
// Free software under the GNU General Public License v3.0 or later;
// see the LICENSE file. There is no warranty, to the extent permitted by law.
//! Neighbourhood graphs, connected components and shortest paths.

use std::cmp::Ordering;
use std::collections::BinaryHeap;

use rayon::prelude::*;

use crate::util::spatial::KdTree;
use crate::Point;

/// Sparse graph in CSR form.
///
/// Edge weights are either stored (`weights`, one per entry of `neighbors`)
/// or, for the directed kNN graphs of [`directed_knn_graph`], computed from
/// the node positions when an edge is visited: a shortest-path search
/// relaxes every edge once, so storing the weight saves no work and costs
/// eight bytes an edge, which on a plot of a few hundred million points is
/// most of the memory.
#[derive(Debug, Clone, Default)]
pub struct Graph<'a> {
    pub offsets: Vec<usize>,
    pub neighbors: Vec<u32>,
    /// One weight per neighbour; empty when `cost` computes them.
    pub weights: Vec<f64>,
    /// Optional node xy positions (needed for the gravity term when the
    /// positions are not in `cost`).
    pub xy: Vec<[f64; 2]>,
    cost: Option<EdgeCost<'a>>,
}

/// The direction-dependent edge cost of [`directed_knn_graph_wood`].
#[derive(Debug, Clone)]
struct EdgeCost<'a> {
    points: &'a [Point],
    power: f64,
    angle_penalty: bool,
    wood: Option<&'a [bool]>,
}

impl EdgeCost<'_> {
    #[inline]
    fn cost(&self, from: usize, to: usize) -> f64 {
        let d = crate::transform::sub(&self.points[to], &self.points[from]);
        let len = crate::transform::norm(&d).max(1e-12);
        let mut c = len.powf(self.power);
        if self.angle_penalty {
            let cos_theta = (d[2] / len).clamp(-1.0, 1.0);
            let deg = cos_theta.acos().to_degrees();
            c *= (0.046051 * deg).exp().min(100.0);
        }
        if let Some(w) = self.wood {
            c *= match (w[from], w[to]) {
                (true, true) => 0.1,
                (false, false) => 20.0,
                (true, false) => 1000.0,
                (false, true) => 1.0,
            };
        }
        c
    }
}

impl<'a> Graph<'a> {
    pub fn n(&self) -> usize {
        self.offsets.len().saturating_sub(1)
    }

    #[inline]
    fn position(&self, i: usize) -> [f64; 2] {
        match &self.cost {
            Some(c) => [c.points[i][0], c.points[i][1]],
            None => self.xy.get(i).copied().unwrap_or([0.0, 0.0]),
        }
    }

    /// Edge weight of the `e`-th entry of `neighbors`, leaving node `i`.
    #[inline]
    fn weight(&self, i: usize, e: usize) -> f64 {
        match &self.cost {
            Some(c) => c.cost(i, self.neighbors[e] as usize),
            None => self.weights[e],
        }
    }

    pub fn edges(&self, i: usize) -> impl Iterator<Item = (usize, f64)> + '_ {
        let (a, b) = (self.offsets[i], self.offsets[i + 1]);
        (a..b).map(move |e| (self.neighbors[e] as usize, self.weight(i, e)))
    }

    /// Build from an adjacency list, symmetrising.
    fn from_adjacency(mut adj: Vec<Vec<(u32, f64)>>) -> Graph<'static> {
        let n = adj.len();
        let mut extra: Vec<Vec<(u32, f64)>> = vec![Vec::new(); n];
        for (i, list) in adj.iter().enumerate() {
            for &(j, w) in list {
                extra[j as usize].push((i as u32, w));
            }
        }
        for (i, list) in adj.iter_mut().enumerate() {
            list.append(&mut extra[i]);
            list.sort_unstable_by(|a, b| a.0.cmp(&b.0));
            list.dedup_by(|a, b| a.0 == b.0);
        }
        graph_from_directed(adj)
    }

    /// The symmetrised neighbour sets of a k-nearest-neighbour search, as
    /// CSR offsets and neighbours: each node's list holds the nodes it
    /// found and the nodes that found it, sorted by index, each once. Built
    /// in flat arrays - a slot table of the search results, a reverse pass,
    /// a sort of each node's run - rather than a vector per node, which for
    /// a few neighbours costs more in headers and slack than in neighbours.
    fn symmetric_knn_csr(points: &[Point], k: usize, max_distance: f64, bridge: bool) -> (Vec<usize>, Vec<u32>) {
        let n = points.len();
        let tree = KdTree::new(points);
        // Up to k + 1 results survive the filter when the point's own entry
        // is not among the k + 1 nearest (coincident points), so a slot
        // table of k + 1 per node holds every list; u32::MAX marks an empty slot.
        let slots = k + 1;
        let mut found = vec![u32::MAX; n * slots];
        found.par_chunks_mut(slots).enumerate().for_each(|(i, row)| {
            let mut m = 0;
            for (j, d) in tree.knn(&points[i], k + 1) {
                if j != i && d <= max_distance {
                    row[m] = j as u32;
                    m += 1;
                }
            }
        });
        drop(tree);
        let extra = if bridge { bridges(points, &found, slots, max_distance) } else { Vec::new() };
        // Degree of each node: what it found plus what found it, and the bridges.
        let mut degree = vec![0usize; n];
        for (i, row) in found.chunks(slots).enumerate() {
            for &j in row {
                if j == u32::MAX {
                    break;
                }
                degree[i] += 1;
                degree[j as usize] += 1;
            }
        }
        for &(i, j) in &extra {
            degree[i as usize] += 1;
            degree[j as usize] += 1;
        }
        let mut offsets = Vec::with_capacity(n + 1);
        offsets.push(0usize);
        for &d in &degree {
            offsets.push(offsets.last().unwrap() + d);
        }
        let mut neighbors = vec![0u32; offsets[n]];
        // Forward entries first, in each node's run, then the reverse ones.
        let mut cursor: Vec<usize> = offsets[..n].to_vec();
        for (i, row) in found.chunks(slots).enumerate() {
            for &j in row {
                if j == u32::MAX {
                    break;
                }
                neighbors[cursor[i]] = j;
                cursor[i] += 1;
            }
        }
        for (i, row) in found.chunks(slots).enumerate() {
            for &j in row {
                if j == u32::MAX {
                    break;
                }
                let j = j as usize;
                neighbors[cursor[j]] = i as u32;
                cursor[j] += 1;
            }
        }
        for &(i, j) in &extra {
            neighbors[cursor[i as usize]] = j;
            cursor[i as usize] += 1;
            neighbors[cursor[j as usize]] = i;
            cursor[j as usize] += 1;
        }
        drop(found);
        drop(cursor);
        // Sort and dedup each run in place; `kept[i]` is its new length.
        let mut kept = vec![0usize; n];
        {
            let mut runs: Vec<&mut [u32]> = Vec::with_capacity(n);
            let mut rest: &mut [u32] = &mut neighbors;
            for i in 0..n {
                let (run, tail) = rest.split_at_mut(degree[i]);
                runs.push(run);
                rest = tail;
            }
            runs.into_par_iter().zip(kept.par_iter_mut()).for_each(|(run, kept)| {
                run.sort_unstable();
                let mut m = 0;
                for e in 0..run.len() {
                    if e == 0 || run[e] != run[e - 1] {
                        run[m] = run[e];
                        m += 1;
                    }
                }
                *kept = m;
            });
        }
        // Close the gaps the duplicates left: each run moves down to the end
        // of the previous one (the runs' old starts are the degree sums).
        let mut write = 0;
        let mut start = 0;
        for i in 0..n {
            if write != start {
                neighbors.copy_within(start..start + kept[i], write);
            }
            write += kept[i];
            start += degree[i];
            offsets[i + 1] = write;
        }
        neighbors.truncate(write);
        neighbors.shrink_to_fit();
        (offsets, neighbors)
    }
}

/// Root of `x` in a union-find forest, halving the path on the way.
fn find(parent: &mut [u32], mut x: u32) -> u32 {
    while parent[x as usize] != x {
        let p = parent[x as usize];
        parent[x as usize] = parent[p as usize];
        x = p;
    }
    x
}

/// The edges that join the connected components of a k-nearest-neighbour
/// graph wherever two of them come within `max_distance`: for every pair
/// of components within reach, the closest pair of nodes between them,
/// repeated until no two components are that close. The graph then
/// connects whatever the cloud connects at `max_distance`, however the
/// points were thinned: a crown joined to its trunk by a few sparsely
/// sampled branches is no longer at the mercy of which point each voxel
/// kept, which is what made a k-neighbour graph fall apart there. A piece
/// near two trees is joined to both, and the path costs decide which it
/// belongs to, as they would in a radius graph. `found` is the slot table
/// of the search, `slots` wide per node.
///
/// The closest pairs come from a search over a bounding-box tree of the
/// nodes in which every box records the component its nodes belong to,
/// when they all belong to one: two boxes of the same component have
/// nothing to offer and are skipped whole, so the inside of a tree costs
/// nothing and the work goes where components meet. A radius search from
/// every node would visit the whole metre around each of them, which on a
/// dense plot is thousands of nodes a node and hours a pass.
fn bridges(points: &[Point], found: &[u32], slots: usize, max_distance: f64) -> Vec<(u32, u32)> {
    let n = points.len();
    let mut parent: Vec<u32> = (0..n as u32).collect();
    for (i, row) in found.chunks(slots).enumerate() {
        for &j in row {
            if j == u32::MAX {
                break;
            }
            let (a, b) = (find(&mut parent, i as u32), find(&mut parent, j));
            if a != b {
                parent[a as usize] = b;
            }
        }
    }
    let mut extra = Vec::new();
    if n == 0 {
        return extra;
    }
    let tree = BoxTree::new(points);
    // Each pass joins every component to its neighbours within reach;
    // chains of components take a few passes, and the count is bounded by
    // the number of components, which halves at least each pass.
    for _ in 0..64 {
        let comp: Vec<u32> = (0..n as u32).map(|i| find(&mut parent, i)).collect();
        if comp.iter().all(|&c| c == comp[0]) {
            break;
        }
        let best = tree.closest_pairs(&comp, max_distance);
        if best.is_empty() {
            break;
        }
        // Every pair's edge goes in (so a piece near two trees reaches
        // both); the unions decide whether another pass is needed.
        let mut joined = false;
        let mut picks: Vec<(f64, u32, u32)> = best.into_values().collect();
        picks.sort_by(|a, b| a.partial_cmp(b).unwrap());
        for (_, i, j) in picks {
            extra.push((i, j));
            let (a, b) = (find(&mut parent, i), find(&mut parent, j));
            if a != b {
                parent[a as usize] = b;
                joined = true;
            }
        }
        if !joined {
            break;
        }
    }
    extra
}

/// The closest pair of nodes per pair of components: `(distance, i, j)`
/// under the component pair `(a, b)` with `a < b`.
type ClosestPairs = std::collections::HashMap<(u32, u32), (f64, u32, u32)>;

/// A bounding-box tree over a point set: a k-d split down to leaves of at
/// most [`BoxTree::LEAF`] points, each node holding the tight box of its
/// points. Built once per graph; the component flags that drive
/// [`BoxTree::closest_pairs`] are recomputed per pass.
struct BoxTree<'a> {
    points: &'a [Point],
    /// Point indices, permuted so that every node holds a contiguous run.
    perm: Vec<u32>,
    nodes: Vec<BoxNode>,
}

#[derive(Clone)]
struct BoxNode {
    lo: Point,
    hi: Point,
    /// Run of `perm` this node holds.
    start: u32,
    end: u32,
    /// First child; the second is `left + 1`. `u32::MAX` for a leaf.
    left: u32,
}

/// Marker for a box whose points span more than one component.
const MIXED: u32 = u32::MAX;

impl<'a> BoxTree<'a> {
    const LEAF: usize = 16;

    fn new(points: &'a [Point]) -> Self {
        let perm: Vec<u32> = (0..points.len() as u32).collect();
        let mut nodes = Vec::with_capacity(2 * points.len() / Self::LEAF + 1);
        let root = BoxNode { lo: [0.0; 3], hi: [0.0; 3], start: 0, end: points.len() as u32, left: u32::MAX };
        nodes.push(root);
        let mut tree = BoxTree { points, perm, nodes };
        tree.build(0);
        tree
    }

    /// Lay out the subtree at `node`: box the run, split the longest axis
    /// at its median point, recurse. Children follow their parent in
    /// `nodes`, so a reverse scan visits every child before its parent.
    fn build(&mut self, node: usize) {
        let (start, end) = (self.nodes[node].start as usize, self.nodes[node].end as usize);
        let (mut lo, mut hi) = ([f64::INFINITY; 3], [f64::NEG_INFINITY; 3]);
        for &i in &self.perm[start..end] {
            for k in 0..3 {
                lo[k] = lo[k].min(self.points[i as usize][k]);
                hi[k] = hi[k].max(self.points[i as usize][k]);
            }
        }
        self.nodes[node].lo = lo;
        self.nodes[node].hi = hi;
        if end - start <= Self::LEAF {
            return;
        }
        let axis = (0..3).max_by(|&a, &b| (hi[a] - lo[a]).total_cmp(&(hi[b] - lo[b]))).unwrap();
        let mid = (start + end) / 2;
        let points = self.points;
        self.perm[start..end].select_nth_unstable_by(mid - start, |&a, &b| points[a as usize][axis].total_cmp(&points[b as usize][axis]));
        let left = self.nodes.len() as u32;
        self.nodes[node].left = left;
        self.nodes.push(BoxNode { lo, hi, start: start as u32, end: mid as u32, left: u32::MAX });
        self.nodes.push(BoxNode { lo, hi, start: mid as u32, end: end as u32, left: u32::MAX });
        self.build(left as usize);
        self.build(left as usize + 1);
    }

    /// The component of each box: its nodes' one component, or `MIXED`.
    fn flags(&self, comp: &[u32]) -> Vec<u32> {
        let mut flag = vec![MIXED; self.nodes.len()];
        for (b, node) in self.nodes.iter().enumerate().rev() {
            flag[b] = if node.left == u32::MAX {
                let mut run = self.perm[node.start as usize..node.end as usize].iter().map(|&i| comp[i as usize]);
                let first = run.next().unwrap_or(MIXED);
                if run.all(|c| c == first) {
                    first
                } else {
                    MIXED
                }
            } else {
                let (l, r) = (flag[node.left as usize], flag[node.left as usize + 1]);
                if l == r {
                    l
                } else {
                    MIXED
                }
            };
        }
        flag
    }

    /// Squared distance between two boxes (zero when they overlap).
    fn gap(&self, a: usize, b: usize) -> f64 {
        let (a, b) = (&self.nodes[a], &self.nodes[b]);
        (0..3)
            .map(|k| {
                let d = (a.lo[k] - b.hi[k]).max(b.lo[k] - a.hi[k]).max(0.0);
                d * d
            })
            .sum()
    }

    /// The closest pair of nodes between every two components that come
    /// within `max_distance`, for the component labels `comp`.
    fn closest_pairs(&self, comp: &[u32], max_distance: f64) -> ClosestPairs {
        let flag = self.flags(comp);
        let limit = max_distance * max_distance;
        // Widen the root pair into enough independent box pairs to share
        // out, then search each and keep the closest per component pair.
        let mut pairs = vec![(0usize, 0usize)];
        let mut next = Vec::new();
        for _ in 0..7 {
            next.clear();
            for &(a, b) in &pairs {
                self.expand(a, b, &flag, limit, &mut next);
            }
            std::mem::swap(&mut pairs, &mut next);
        }
        pairs
            .par_iter()
            .fold(ClosestPairs::new, |mut best, &(a, b)| {
                self.search(a, b, comp, &flag, limit, &mut best);
                best
            })
            .reduce(ClosestPairs::new, |mut x, y| {
                for (key, v) in y {
                    let e = x.entry(key).or_insert(v);
                    if v < *e {
                        *e = v;
                    }
                }
                x
            })
    }

    /// Whether the pair of boxes can hold a bridge at all.
    fn worth(&self, a: usize, b: usize, flag: &[u32], limit: f64) -> bool {
        !(flag[a] != MIXED && flag[a] == flag[b]) && self.gap(a, b) <= limit
    }

    /// The child pairs of a box pair, or the pair itself when it has no
    /// children to open, dropping what `worth` rules out.
    fn expand(&self, a: usize, b: usize, flag: &[u32], limit: f64, out: &mut Vec<(usize, usize)>) {
        if !self.worth(a, b, flag, limit) {
            return;
        }
        let (la, lb) = (self.nodes[a].left, self.nodes[b].left);
        match (la != u32::MAX, lb != u32::MAX) {
            (false, false) => out.push((a, b)),
            (true, false) => out.extend([(la as usize, b), (la as usize + 1, b)]),
            (false, true) => out.extend([(a, lb as usize), (a, lb as usize + 1)]),
            (true, true) if a == b => out.extend([(la as usize, la as usize), (la as usize + 1, la as usize + 1), (la as usize, la as usize + 1)]),
            (true, true) => out.extend([(la as usize, lb as usize), (la as usize, lb as usize + 1), (la as usize + 1, lb as usize), (la as usize + 1, lb as usize + 1)]),
        }
    }

    /// Search a pair of boxes for closest pairs between components, into `best`.
    fn search(&self, a: usize, b: usize, comp: &[u32], flag: &[u32], limit: f64, best: &mut ClosestPairs) {
        if !self.worth(a, b, flag, limit) {
            return;
        }
        if flag[a] != MIXED && flag[b] != MIXED {
            // Two boxes of two components: nothing to gain past the pair
            // already found for them.
            let key = if flag[a] < flag[b] { (flag[a], flag[b]) } else { (flag[b], flag[a]) };
            if best.get(&key).is_some_and(|e| e.0 * e.0 <= self.gap(a, b)) {
                return;
            }
        }
        let (la, lb) = (self.nodes[a].left, self.nodes[b].left);
        if la == u32::MAX && lb == u32::MAX {
            let (na, nb) = (&self.nodes[a], &self.nodes[b]);
            for &i in &self.perm[na.start as usize..na.end as usize] {
                let ci = comp[i as usize];
                for &j in &self.perm[nb.start as usize..nb.end as usize] {
                    if a == b && j <= i {
                        continue;
                    }
                    let cj = comp[j as usize];
                    if ci == cj {
                        continue;
                    }
                    let (p, q) = (&self.points[i as usize], &self.points[j as usize]);
                    let d2 = (0..3).map(|k| (p[k] - q[k]) * (p[k] - q[k])).sum::<f64>();
                    if d2 > limit {
                        continue;
                    }
                    let (key, i, j) = if ci < cj { ((ci, cj), i, j) } else { ((cj, ci), j, i) };
                    let d = d2.sqrt();
                    let e = best.entry(key).or_insert((f64::INFINITY, 0, 0));
                    if d < e.0 {
                        *e = (d, i, j);
                    }
                }
            }
            return;
        }
        let mut children = Vec::with_capacity(4);
        self.expand(a, b, flag, limit, &mut children);
        for (x, y) in children {
            self.search(x, y, comp, flag, limit, best);
        }
    }

}

/// Directed graph from per-node adjacency lists (no symmetrisation).
pub fn graph_from_directed(adj: Vec<Vec<(u32, f64)>>) -> Graph<'static> {
    let mut offsets = Vec::with_capacity(adj.len() + 1);
    let mut neighbors = Vec::new();
    let mut weights = Vec::new();
    offsets.push(0);
    for list in &adj {
        for &(j, w) in list {
            neighbors.push(j);
            weights.push(w);
        }
        offsets.push(neighbors.len());
    }
    Graph { offsets, neighbors, weights, xy: Vec::new(), cost: None }
}

/// What a k-nearest-neighbour graph over `n` points will cost, and whether it
/// fits. The symmetrised neighbour lists hold up to `2k + 2` entries a node
/// as `u32`, built from a slot table of `k + 1` a node, with the degrees,
/// offsets and the kd-tree's copy of the points beside them (about 60 bytes
/// a node); a graph with stored weights adds an `f64` an entry.
fn check_graph(n: usize, k: usize, stored_weights: bool) -> crate::error::Result<()> {
    let per_edge = std::mem::size_of::<u32>() as u64 + if stored_weights { std::mem::size_of::<f64>() as u64 } else { 0 };
    let edges = (n as u128) * (3 * k as u128 + 3);
    let bytes = edges.saturating_mul(per_edge as u128).saturating_add((n as u128) * 60);
    crate::util::limits::check_cells(
        bytes,
        1,
        &format!("a {k}-neighbour graph over {n} points"),
        "thinning the cloud (filters.voxel_downsample), a smaller k, or working tile by tile",
    )
}

/// Directed kNN graph with a direction-dependent cost:
/// `cost(from -> to) = d^power * penalty(angle from vertical)`, where the
/// penalty is `min(exp(0.046051 * deg), 100)` -- travelling up is free,
/// horizontal costs x63 and downward x100 -- so shortest paths from stem
/// seeds climb through the tree rather than leaking across the ground.
/// The neighbour set is symmetrised so the graph is connected both ways.
pub fn directed_knn_graph(points: &[Point], k: usize, max_distance: f64, power: f64, angle_penalty: bool) -> Graph<'_> {
    directed_knn_graph_wood(points, k, max_distance, power, angle_penalty, None)
}

/// [`directed_knn_graph`] with wood / leaf cost factors when a per-node
/// `wood` mask is given: wood->wood x0.1, leaf->leaf x20, wood->leaf x1000,
/// leaf->wood x1. Paths run along the woody skeleton and may enter it from
/// foliage, but almost never leave it, so foliage cannot bridge two trees.
///
/// The graph borrows `points` (and `wood`): edge costs are computed from
/// them as the edges are walked, not stored.
pub fn directed_knn_graph_wood<'a>(points: &'a [Point], k: usize, max_distance: f64, power: f64, angle_penalty: bool, wood: Option<&'a [bool]>) -> Graph<'a> {
    directed_knn_graph_full(points, k, max_distance, power, angle_penalty, wood, false)
}

/// [`directed_knn_graph_wood`] with its components joined wherever two come
/// within `max_distance` of each other (see `bridges`), so that connectivity
/// is that of the cloud at `max_distance` and not of the thinning.
pub fn directed_knn_graph_bridged<'a>(points: &'a [Point], k: usize, max_distance: f64, power: f64, angle_penalty: bool, wood: Option<&'a [bool]>) -> Graph<'a> {
    directed_knn_graph_full(points, k, max_distance, power, angle_penalty, wood, true)
}

fn directed_knn_graph_full<'a>(points: &'a [Point], k: usize, max_distance: f64, power: f64, angle_penalty: bool, wood: Option<&'a [bool]>, bridge: bool) -> Graph<'a> {
    if let Err(e) = check_graph(points.len(), k, false) {
        // The callers of this one cannot carry an error; refusing loudly is
        // still better than the machine going down.
        panic!("{e}");
    }
    let (offsets, neighbors) = Graph::symmetric_knn_csr(points, k, max_distance, bridge);
    Graph { offsets, neighbors, weights: Vec::new(), xy: Vec::new(), cost: Some(EdgeCost { points, power, angle_penalty, wood }) }
}

/// Symmetric k-nearest-neighbour graph; edges longer than `max_distance` are dropped.
pub fn knn_graph(points: &[Point], k: usize, max_distance: f64) -> Graph<'static> {
    if let Err(e) = check_graph(points.len(), k, true) {
        panic!("{e}");
    }
    let tree = KdTree::new(points);
    let adj: Vec<Vec<(u32, f64)>> = points
        .par_iter()
        .enumerate()
        .map(|(i, p)| {
            tree.knn(p, k + 1)
                .into_iter()
                .filter(|&(j, d)| j != i && d <= max_distance)
                .map(|(j, d)| (j as u32, d.max(1e-12)))
                .collect()
        })
        .collect();
    Graph::from_adjacency(adj)
}

/// Graph linking all point pairs closer than `radius`.
pub fn radius_graph(points: &[Point], radius: f64) -> Graph<'static> {
    let tree = KdTree::new(points);
    let adj: Vec<Vec<(u32, f64)>> = points
        .par_iter()
        .enumerate()
        .map(|(i, p)| {
            tree.within(p, radius)
                .into_iter()
                .filter(|&(j, _)| j != i)
                .map(|(j, d)| (j as u32, d.max(1e-12)))
                .collect()
        })
        .collect();
    Graph::from_adjacency(adj)
}

/// Connected component label per node (0-based, arbitrary order).
pub fn connected_components(graph: &Graph) -> (usize, Vec<usize>) {
    let n = graph.n();
    let mut label = vec![usize::MAX; n];
    let mut count = 0;
    let mut stack = Vec::new();
    for s in 0..n {
        if label[s] != usize::MAX {
            continue;
        }
        label[s] = count;
        stack.push(s);
        while let Some(i) = stack.pop() {
            for (j, _) in graph.edges(i) {
                if label[j] == usize::MAX {
                    label[j] = count;
                    stack.push(j);
                }
            }
        }
        count += 1;
    }
    (count, label)
}

/// Euclidean clustering: connected components of the radius graph.
///
/// Returns a label per point; clusters smaller than `min_points` get `-1`.
/// Labels are ordered by decreasing cluster size.
pub fn euclidean_clusters(points: &[Point], radius: f64, min_points: usize) -> Vec<i64> {
    if points.is_empty() {
        return Vec::new();
    }
    let (count, labels) = connected_components(&radius_graph(points, radius));
    let mut sizes = vec![0usize; count];
    for &l in &labels {
        sizes[l] += 1;
    }
    let mut order: Vec<usize> = (0..count).collect();
    order.sort_by_key(|&c| std::cmp::Reverse(sizes[c]));
    let mut remap = vec![-1i64; count];
    let mut next = 0;
    for c in order {
        if sizes[c] >= min_points {
            remap[c] = next;
            next += 1;
        }
    }
    labels.iter().map(|&l| remap[l]).collect()
}

#[derive(Copy, Clone, PartialEq)]
struct HeapItem {
    dist: f64,
    node: usize,
}

impl Eq for HeapItem {}

impl Ord for HeapItem {
    fn cmp(&self, other: &Self) -> Ordering {
        other.dist.partial_cmp(&self.dist).unwrap_or(Ordering::Equal)
    }
}

impl PartialOrd for HeapItem {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

/// Multi-source Dijkstra.
///
/// Returns per node `(distance, source index into `sources`, predecessor)`;
/// unreachable nodes have infinite distance and `usize::MAX` markers.
pub fn dijkstra(graph: &Graph, sources: &[usize]) -> (Vec<f64>, Vec<usize>, Vec<usize>) {
    dijkstra_gravity(graph, sources, None, 0.0)
}

/// [`dijkstra`] with raycloudtools' gravity term (Devereux et al. 2026): each edge cost is scaled by
/// `1 + gravity * lateral^2`, the squared horizontal offset of the node being
/// expanded from the seed its path started at (`seed_xy[source]`), so long
/// horizontal reaches away from a stem are discouraged.
pub fn dijkstra_gravity(graph: &Graph, sources: &[usize], seed_xy: Option<&[[f64; 2]]>, gravity: f64) -> (Vec<f64>, Vec<usize>, Vec<usize>) {
    dijkstra_scaled(graph, sources, seed_xy, gravity, None)
}

/// [`dijkstra_gravity`] with an additional per-source cost multiplier
/// (`source_scale[source]`), raycloudtools' `score /= node.radius`: giving
/// taller trees a smaller scale lets them win contested crown points.
pub fn dijkstra_scaled(graph: &Graph, sources: &[usize], seed_xy: Option<&[[f64; 2]]>, gravity: f64, source_scale: Option<&[f64]>) -> (Vec<f64>, Vec<usize>, Vec<usize>) {
    let n = graph.n();
    let mut dist = vec![f64::INFINITY; n];
    let mut src = vec![usize::MAX; n];
    let mut pred = vec![usize::MAX; n];
    let mut heap = BinaryHeap::new();
    for (si, &s) in sources.iter().enumerate() {
        if dist[s] > 0.0 {
            dist[s] = 0.0;
            src[s] = si;
            heap.push(HeapItem { dist: 0.0, node: s });
        }
    }
    while let Some(HeapItem { dist: d, node: i }) = heap.pop() {
        if d > dist[i] {
            continue;
        }
        let mut scale = match (seed_xy, gravity > 0.0) {
            (Some(sxy), true) if src[i] != usize::MAX => {
                let s = sxy[src[i]];
                let p = graph.position(i);
                let (dx, dy) = (p[0] - s[0], p[1] - s[1]);
                1.0 + gravity * (dx * dx + dy * dy)
            }
            _ => 1.0,
        };
        if let Some(ss) = source_scale {
            if src[i] != usize::MAX {
                scale *= ss[src[i]];
            }
        }
        for (j, w) in graph.edges(i) {
            let nd = d + w * scale;
            if nd < dist[j] {
                dist[j] = nd;
                src[j] = src[i];
                pred[j] = i;
                heap.push(HeapItem { dist: nd, node: j });
            }
        }
    }
    (dist, src, pred)
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::filters::Rng;

    /// The graph as it was built before the flat CSR: a vector per node,
    /// symmetrised, sorted and deduplicated, with the costs stored.
    fn reference(points: &[Point], k: usize, max_distance: f64, power: f64, angle_penalty: bool, wood: Option<&[bool]>) -> Graph<'static> {
        let tree = KdTree::new(points);
        let mut adj: Vec<Vec<u32>> = points.iter().enumerate().map(|(i, p)| tree.knn(p, k + 1).into_iter().filter(|&(j, d)| j != i && d <= max_distance).map(|(j, _)| j as u32).collect()).collect();
        let mut extra: Vec<Vec<u32>> = vec![Vec::new(); points.len()];
        for (i, list) in adj.iter().enumerate() {
            for &j in list {
                extra[j as usize].push(i as u32);
            }
        }
        for (i, list) in adj.iter_mut().enumerate() {
            list.append(&mut extra[i]);
            list.sort_unstable();
            list.dedup();
        }
        let cost = EdgeCost { points, power, angle_penalty, wood };
        let weighted: Vec<Vec<(u32, f64)>> = adj.into_iter().enumerate().map(|(i, list)| list.into_iter().map(|j| (j, cost.cost(i, j as usize))).collect()).collect();
        let mut g = graph_from_directed(weighted);
        g.xy = points.iter().map(|p| [p[0], p[1]]).collect();
        g
    }

    fn cloud(n: usize, seed: u64, coincident: bool) -> Vec<Point> {
        let mut rng = Rng::new(seed);
        let mut unit = || (rng.next_u64() >> 11) as f64 / (1u64 << 53) as f64;
        let mut pts: Vec<Point> = (0..n).map(|_| [4.0 * unit(), 4.0 * unit(), 6.0 * unit()]).collect();
        if coincident {
            // Clumps of identical points, so a point's own entry can fall
            // outside its k + 1 nearest.
            for i in 0..n / 4 {
                pts[n / 2 + i] = pts[i];
                pts[n / 4 * 3 + i % (n / 4)] = pts[i];
            }
        }
        pts
    }

    fn same(a: &Graph<'_>, b: &Graph<'_>) {
        assert_eq!(a.offsets, b.offsets);
        assert_eq!(a.neighbors, b.neighbors);
        for i in 0..a.n() {
            let ea: Vec<(usize, f64)> = a.edges(i).collect();
            let eb: Vec<(usize, f64)> = b.edges(i).collect();
            assert_eq!(ea.len(), eb.len());
            for (x, y) in ea.iter().zip(&eb) {
                assert_eq!(x.0, y.0);
                assert!(x.1.to_bits() == y.1.to_bits(), "node {i}: {} vs {}", x.1, y.1);
            }
            assert_eq!(a.position(i), b.position(i));
        }
    }

    #[test]
    fn flat_csr_matches_the_vector_build() {
        for (seed, coincident, max_edge) in [(1, false, 1.0), (2, true, 1.0), (3, false, 0.3), (4, true, 0.05)] {
            let pts = cloud(400, seed, coincident);
            let wood: Vec<bool> = pts.iter().map(|p| p[2] > 3.0).collect();
            for w in [None, Some(wood.as_slice())] {
                let g = directed_knn_graph_wood(&pts, 6, max_edge, 6.0, true, w);
                same(&g, &reference(&pts, 6, max_edge, 6.0, true, w));
            }
            let g = directed_knn_graph(&pts, 10, max_edge, 3.0, false);
            same(&g, &reference(&pts, 10, max_edge, 3.0, false, None));
        }
    }

    #[test]
    fn paths_are_those_of_the_stored_graph() {
        let pts = cloud(600, 9, true);
        let g = directed_knn_graph(&pts, 6, 1.0, 6.0, true);
        let r = reference(&pts, 6, 1.0, 6.0, true, None);
        let sources = [0usize, 7, 123];
        let xy = [[pts[0][0], pts[0][1]], [pts[7][0], pts[7][1]], [pts[123][0], pts[123][1]]];
        let scale = [0.5, 1.0, 0.25];
        let a = dijkstra_scaled(&g, &sources, Some(&xy), 0.3, Some(&scale));
        let b = dijkstra_scaled(&r, &sources, Some(&xy), 0.3, Some(&scale));
        assert!(a.0.iter().zip(&b.0).all(|(x, y)| x.to_bits() == y.to_bits()));
        assert_eq!(a.1, b.1);
        assert_eq!(a.2, b.2);
        assert_eq!(connected_components(&g), connected_components(&r));
    }

    #[test]
    fn bridges_join_what_the_cloud_joins() {
        // Two dense vertical lines 0.8 m apart: every node's six nearest are
        // on its own line, so the kNN graph has two components; bridged within
        // 1 m it has one, within 0.5 m still two.
        let mut pts: Vec<Point> = (0..200).map(|i| [0.0, 0.0, i as f64 * 0.02]).collect();
        pts.extend((0..200).map(|i| [0.8, 0.0, i as f64 * 0.02]));
        let plain = directed_knn_graph(&pts, 6, 1.0, 6.0, true);
        assert_eq!(connected_components(&plain).0, 2);
        let bridged = directed_knn_graph_bridged(&pts, 6, 1.0, 6.0, true, None);
        assert_eq!(connected_components(&bridged).0, 1);
        assert_eq!(bridged.neighbors.len(), plain.neighbors.len() + 2);
        let apart = directed_knn_graph_bridged(&pts, 6, 0.5, 6.0, true, None);
        assert_eq!(connected_components(&apart).0, 2);
        // A chain of four clumps joins in passes, and a lone far point stays apart.
        let mut chain: Vec<Point> = Vec::new();
        for c in 0..4 {
            chain.extend((0..50).map(|i| [c as f64 * 0.9, 0.0, i as f64 * 0.01]));
        }
        chain.push([10.0, 10.0, 0.0]);
        assert_eq!(connected_components(&directed_knn_graph_bridged(&chain, 6, 1.0, 6.0, true, None)).0, 2);
        // Bridging an already connected graph adds nothing.
        let dense = cloud(300, 4, false);
        let same = directed_knn_graph_bridged(&dense, 6, 1.0, 6.0, true, None);
        let base = directed_knn_graph(&dense, 6, 1.0, 6.0, true);
        assert_eq!(connected_components(&base).0, 1);
        assert_eq!(same.neighbors, base.neighbors);
    }

    #[test]
    fn empty_and_tiny_graphs() {
        let g = directed_knn_graph(&[], 6, 1.0, 6.0, true);
        assert_eq!(g.n(), 0);
        let one = [[0.0, 0.0, 0.0]];
        let g = directed_knn_graph(&one, 6, 1.0, 6.0, true);
        assert_eq!(g.n(), 1);
        assert_eq!(g.edges(0).count(), 0);
    }
}