sylva-rs 2.0.0

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
// 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.
//! Foliage for a structure model: leaf angle distribution, leaf area from
//! leaf points, and leaf meshes placed on a QSM.
//!
//! A QSM describes the wood. To make a whole-tree model for radiative
//! transfer or visualisation the leaves are added as flat polygons whose
//! total area, spatial distribution and orientation follow what was measured:
//!
//! * **angles** -- the inclination of a leaf is the angle between its normal
//!   and the vertical; normals come from a PCA of each leaf point's
//!   neighbours, their inclinations are binned over `[0, 90]` degrees and
//!   summarised by the mean, a two-parameter beta distribution (Goel &
//!   Strebel 1984), Campbell's ellipsoidal `chi` (1990) and the nearest de
//!   Wit (1965) type;
//! * **area** -- a leaf area per voxel, either from ray-traced leaf area
//!   density (`voxel`) or, without pulses, from the leaf points themselves: a
//!   surface thinned to one point per cube of side `res` crosses
//!   `(|nx| + |ny| + |nz|) / res^2` cubes per unit area, so each thinned point
//!   stands for `res^2 / (|nx| + |ny| + |nz|)` of surface. That counts what
//!   the scanner saw: occluded foliage is missing, so it is a lower bound;
//! * **insertion** -- each voxel receives leaves until its area is met,
//!   centred on leaf points of that voxel (or uniformly inside it), with
//!   normals drawn from the angle distribution and a uniform azimuth, and the
//!   blade pointing away from the nearest branch of the QSM. Leaves may
//!   intersect each other; nothing here resolves collisions (unlike the
//!   leaf insertion of Ã…kerblom et al. 2018).

pub mod model;

use std::collections::HashMap;
use std::f64::consts::{FRAC_PI_2, PI, TAU};

use rayon::prelude::*;

use crate::filters::{local_pca, voxel_downsample_indices, Rng};
use crate::qsm::model::Cylinder;
use crate::qsm::wood::{wood_mask, WoodParams};
use crate::util::spatial::KdTree;
use crate::transform::{add, cross, dot, norm, normalize, scale, sub};
use crate::voxel::metrics::classify_de_wit;
use crate::Point;

/// Inclination (rad, `[0, pi/2]`) of the local surface normal at every point,
/// from a PCA over `k` neighbours, with the normal itself.
pub fn inclinations(points: &[Point], k: usize) -> (Vec<f64>, Vec<Point>) {
    let (normals, _) = local_pca(points, k.max(3));
    let incl = normals.iter().map(|n| n[2].abs().min(1.0).acos()).collect();
    (incl, normals)
}

/// A leaf inclination angle distribution.
#[derive(Debug, Clone, PartialEq)]
pub struct LeafAngles {
    /// Bin centres (rad) over `[0, pi/2]`.
    pub bin_centres: Vec<f64>,
    /// Probability per bin (sums to 1).
    pub density: Vec<f64>,
    /// Mean inclination (rad).
    pub mean: f64,
    /// Standard deviation of the inclination (rad).
    pub std: f64,
    /// Beta distribution on `t = 2 theta / pi`: `f(t) ~ t^(a-1) (1-t)^(b-1)`,
    /// which is Goel & Strebel's (1984) `(mu, nu) = (beta_b, beta_a)`, the
    /// order of [`crate::voxel::Lad::TwoParamBeta`].
    pub beta_a: f64,
    /// Second beta parameter (see `beta_a`).
    pub beta_b: f64,
    /// Campbell's ellipsoidal parameter (1 spherical, > 1 planophile).
    pub chi: f64,
    /// Nearest de Wit (1965) type by name; `None` when the histogram is empty.
    pub de_wit: Option<&'static str>,
}

/// Bin inclinations (rad), optionally weighted (e.g. by the area each point
/// stands for), and fit the summary parameters.
pub fn angle_distribution(inclination: &[f64], weights: Option<&[f64]>, n_bins: usize) -> LeafAngles {
    let n_bins = n_bins.max(1);
    let mut density = vec![0.0; n_bins];
    let (mut sw, mut s1, mut s2) = (0.0, 0.0, 0.0);
    for (i, &th) in inclination.iter().enumerate() {
        if !th.is_finite() {
            continue;
        }
        let w = weights.map_or(1.0, |w| w[i]);
        let th = th.clamp(0.0, FRAC_PI_2);
        density[((th / FRAC_PI_2 * n_bins as f64) as usize).min(n_bins - 1)] += w;
        sw += w;
        s1 += w * th;
        s2 += w * th * th;
    }
    let bin_centres: Vec<f64> = (0..n_bins).map(|b| (b as f64 + 0.5) * FRAC_PI_2 / n_bins as f64).collect();
    if sw <= 0.0 {
        // Nothing to go on: spherical, whose density is sin(theta).
        let mut d: Vec<f64> = bin_centres.iter().map(|t| t.sin()).collect();
        let s: f64 = d.iter().sum();
        d.iter_mut().for_each(|v| *v /= s);
        return LeafAngles { bin_centres, density: d, mean: 1.0, std: 0.0, beta_a: f64::NAN, beta_b: f64::NAN, chi: 1.0, de_wit: Some("spherical") };
    }
    density.iter_mut().for_each(|v| *v /= sw);
    let mean = s1 / sw;
    let var = (s2 / sw - mean * mean).max(0.0);
    let t = mean / FRAC_PI_2;
    let vt = var / (FRAC_PI_2 * FRAC_PI_2);
    let c = if vt > 0.0 { t * (1.0 - t) / vt - 1.0 } else { f64::NAN };
    // Campbell (1990): mean inclination = 9.65 (3 + chi)^-1.65 rad.
    let chi = ((mean.max(1e-6) / 9.65).powf(-1.0 / 1.65) - 3.0).max(0.01);
    let de_wit = classify_de_wit(&bin_centres, &density);
    LeafAngles { bin_centres, density, mean, std: var.sqrt(), beta_a: t * c, beta_b: (1.0 - t) * c, chi, de_wit }
}

impl LeafAngles {
    fn unit(rng: &mut Rng) -> f64 {
        (rng.next_u64() >> 11) as f64 / (1u64 << 53) as f64
    }

    /// Draw an inclination (rad): a bin by its probability, uniform within it.
    fn sample(&self, rng: &mut Rng) -> f64 {
        let u = Self::unit(rng);
        let mut acc = 0.0;
        let n = self.density.len();
        let width = FRAC_PI_2 / n as f64;
        for (b, &p) in self.density.iter().enumerate() {
            acc += p;
            if u <= acc || b == n - 1 {
                return (b as f64 + Self::unit(rng)) * width;
            }
        }
        FRAC_PI_2 / 2.0
    }
}

/// Leaf area seen in the points: thin them to one per cube of side `res`,
/// estimate each survivor's normal over `k` neighbours, and give it the area
/// `res^2 / (|nx| + |ny| + |nz|)`. Returns the thinned points, their area and
/// their inclination (rad).
///
/// `res <= 0` picks it from the data: 3.5 times the median nearest-neighbour
/// spacing. The estimate is a box count and has no plateau: smaller cubes fall
/// between the points and undercount, larger ones overcount at leaf edges and
/// through the range noise. On unoccluded synthetic leaves of 2-8 cm the
/// default is within about 15 % from 5 000 to 80 000 points per m2.
pub fn point_leaf_area(points: &[Point], res: f64, k: usize) -> (Vec<Point>, Vec<f64>, Vec<f64>) {
    let res = if res > 0.0 { res } else { 3.5 * median_spacing(points) };
    if !(res > 0.0) {
        return (Vec::new(), Vec::new(), Vec::new());
    }
    let keep = voxel_downsample_indices(points, res);
    let thin: Vec<Point> = keep.iter().map(|&i| points[i]).collect();
    if thin.len() < 3 {
        return (thin, Vec::new(), Vec::new());
    }
    let (incl, normals) = inclinations(&thin, k.min(thin.len()));
    let area = normals.iter().map(|n| res * res / (n[0].abs() + n[1].abs() + n[2].abs()).max(1.0)).collect();
    (thin, area, incl)
}

/// Median distance to the nearest neighbour, from up to 20 000 evenly taken points.
pub fn median_spacing(points: &[Point]) -> f64 {
    if points.len() < 2 {
        return 0.0;
    }
    let tree = KdTree::new(points);
    let step = points.len().div_ceil(20_000);
    let mut d: Vec<f64> = points.par_iter().step_by(step).filter_map(|p| tree.knn(p, 2).get(1).map(|&(_, d)| d)).collect();
    if d.is_empty() {
        return 0.0;
    }
    d.sort_by(|a, b| a.partial_cmp(b).unwrap());
    d[d.len() / 2]
}

/// Wood (`true`) / leaf (`false`) for every point: the classification runs on
/// the cloud thinned to `voxel_size` and each point takes the label of its
/// nearest thinned neighbour.
pub fn classify_leaf_wood(points: &[Point], voxel_size: f64, params: &WoodParams) -> Vec<bool> {
    classify_thinned(points, voxel_size, |thin| wood_mask(thin, params))
}

/// The same with the graph-based separation ([`crate::qsm::wood::gbs_mask`]).
pub fn classify_leaf_wood_gbs(points: &[Point], voxel_size: f64, params: &crate::qsm::wood::GbsParams) -> Vec<bool> {
    classify_thinned(points, voxel_size, |thin| crate::qsm::wood::gbs_mask(thin, params))
}

/// [`classify_leaf_wood`] with the filter's per-point cues
/// ([`crate::qsm::wood::WoodScores`]), each point taking its nearest thinned
/// point's values.
pub fn classify_leaf_wood_scores(points: &[Point], voxel_size: f64, params: &WoodParams) -> crate::qsm::wood::WoodScores {
    let (thin, near) = thinned(points, voxel_size);
    let s = crate::qsm::wood::wood_scores(&thin, params);
    crate::qsm::wood::WoodScores { mask: near.iter().map(|&j| s.mask[j]).collect(), anisotropy: near.iter().map(|&j| s.anisotropy[j]).collect(), support: near.iter().map(|&j| s.support[j]).collect() }
}

/// [`classify_leaf_wood_gbs`] with the share of shell scales at which each
/// point was wood ([`crate::qsm::wood::gbs_scores`]).
pub fn classify_leaf_wood_gbs_scores(points: &[Point], voxel_size: f64, params: &crate::qsm::wood::GbsParams) -> (Vec<bool>, Vec<f64>) {
    let (thin, near) = thinned(points, voxel_size);
    let (mask, votes) = crate::qsm::wood::gbs_scores(&thin, params);
    (near.iter().map(|&j| mask[j]).collect(), near.iter().map(|&j| votes[j]).collect())
}

/// Wood confidence in `[0, 1]` from the path filter's cues: 1 where the
/// filter keeps the point, and `DROPPED_WEIGHT` times the anisotropy where it
/// drops it, so a dropped point still counts a little towards a circle,
/// in proportion to how planar or linear its neighbourhood is, but is
/// never confident wood (below 0.5).
pub fn path_confidence(s: &crate::qsm::wood::WoodScores) -> Vec<f64> {
    s.mask.iter().zip(&s.anisotropy).map(|(&m, &a)| if m { 1.0 } else { DROPPED_WEIGHT * a.clamp(0.0, 1.0) }).collect()
}

/// Largest weight [`path_confidence`] gives a point the filter drops.
pub const DROPPED_WEIGHT: f64 = 0.4;

/// Wood confidence in `[0, 1]` from the graph-based labels: half for the
/// label, half for the share of shell scales at which the point's piece was
/// wood. A point that is wood at every scale scores 1, one that is wood only
/// by being on a path or next to wood 0.5, and a leaf 0.
pub fn gbs_confidence(mask: &[bool], votes: &[f64]) -> Vec<f64> {
    mask.iter().zip(votes).map(|(&m, &v)| (0.5 * m as u8 as f64 + 0.5 * v).clamp(0.0, 1.0)).collect()
}

/// The cloud thinned to `voxel_size` (every point if not positive) and each
/// input point's nearest thinned point.
fn thinned(points: &[Point], voxel_size: f64) -> (Vec<Point>, Vec<usize>) {
    if points.is_empty() || voxel_size.is_nan() || voxel_size <= 0.0 {
        return (points.to_vec(), (0..points.len()).collect());
    }
    let keep = voxel_downsample_indices(points, voxel_size);
    let thin: Vec<Point> = keep.iter().map(|&i| points[i]).collect();
    let tree = KdTree::new(&thin);
    let near = points.par_iter().map(|p| tree.nearest(p).map(|(j, _)| j).unwrap_or(0)).collect();
    (thin, near)
}

fn classify_thinned(points: &[Point], voxel_size: f64, classify: impl Fn(&[Point]) -> Vec<bool>) -> Vec<bool> {
    if points.is_empty() {
        return Vec::new();
    }
    if !(voxel_size > 0.0) {
        return classify(points);
    }
    let keep = voxel_downsample_indices(points, voxel_size);
    let thin: Vec<Point> = keep.iter().map(|&i| points[i]).collect();
    let mask = classify(&thin);
    let tree = KdTree::new(&thin);
    points.par_iter().map(|p| tree.nearest(p).map(|(j, _)| mask[j]).unwrap_or(false)).collect()
}

#[derive(Debug, Clone)]
/// Shape and placement rules of the leaves [`insert_leaves`] puts down.
pub struct LeafParams {
    /// Blade length and greatest width (m).
    pub length: f64,
    /// Greatest blade width (m).
    pub width: f64,
    /// Shape of one blade, in unit leaf space.
    pub blade: LeafBlade,
    /// Leaves whose centre is within this distance of a cylinder axis point
    /// away from it; further ones take a random in-plane direction.
    pub max_branch_distance: f64,
    /// Random displacement of a leaf centre from its seed point (m).
    pub jitter: f64,
    /// Seed of the random draws (positions, angles, blade directions).
    pub seed: u64,
}

impl Default for LeafParams {
    fn default() -> Self {
        LeafParams { length: 0.08, width: 0.04, blade: LeafBlade::default(), max_branch_distance: 0.5, jitter: 0.01, seed: 1 }
    }
}

/// A blade in unit leaf space: `(along, across, up)`, base at the origin, tip
/// at `along = 1`, greatest width 1 across. A placed leaf scales `along` by
/// the length and `across` and `up` by the width, so a blade may be curled
/// or made of several leaflets, not only a flat outline.
#[derive(Debug, Clone)]
pub struct LeafBlade {
    /// Vertices as `(along, across, up)` in unit leaf space.
    pub vertices: Vec<Point>,
    /// Triangles as indices into `vertices`.
    pub faces: Vec<[u32; 3]>,
}

/// Outline of the built-in unit leaf in its own plane, base at the origin, tip
/// at `(1, 0)`, half-width 0.5 at 45 % of the length: `(along, across)`.
const OUTLINE: [(f64, f64); 6] = [(0.0, 0.0), (0.2, 0.36), (0.45, 0.5), (1.0, 0.0), (0.45, -0.5), (0.2, -0.36)];

impl Default for LeafBlade {
    fn default() -> Self {
        LeafBlade {
            vertices: OUTLINE.iter().map(|&(x, y)| [x, y, 0.0]).collect(),
            faces: (1..OUTLINE.len() as u32 - 1).map(|t| [0, t, t + 1]).collect(),
        }
    }
}

impl LeafBlade {
    /// One-sided area of this blade at the given length and width: the sum of
    /// its triangles, so a flat outline gives the area it encloses.
    pub fn area(&self, length: f64, width: f64) -> f64 {
        let s = |v: &Point| [v[0] * length, v[1] * width, v[2] * width];
        self.faces
            .iter()
            .map(|f| {
                let (a, b, c) = (s(&self.vertices[f[0] as usize]), s(&self.vertices[f[1] as usize]), s(&self.vertices[f[2] as usize]));
                0.5 * norm(&cross(&sub(&b, &a), &sub(&c, &a)))
            })
            .sum()
    }
}

/// One-sided area of a leaf of the given length and width, for the built-in blade.
pub fn single_leaf_area(length: f64, width: f64) -> f64 {
    LeafBlade::default().area(length, width)
}

#[derive(Debug, Clone, Default)]
/// The leaves placed by [`insert_leaves`]: one triangle mesh plus a record per leaf.
pub struct LeafMesh {
    /// Vertices of every leaf (m).
    pub vertices: Vec<Point>,
    /// Triangles as indices into `vertices`.
    pub faces: Vec<[u32; 3]>,
    /// Per leaf: centre, unit normal, inclination (rad), nearest cylinder (-1 if none in reach).
    pub centres: Vec<Point>,
    /// Unit normal of each leaf.
    pub normals: Vec<Point>,
    /// Inclination of each leaf (rad, 0 horizontal).
    pub inclination: Vec<f64>,
    /// Index of the nearest QSM cylinder of each leaf, −1 when none within `max_branch_distance`.
    pub cylinder: Vec<i64>,
    /// One-sided area of one leaf (m2).
    pub leaf_area: f64,
}

impl LeafMesh {
    /// Leaves placed.
    pub fn n_leaves(&self) -> usize {
        self.centres.len()
    }

    /// One-sided area of all leaves (m²).
    pub fn total_area(&self) -> f64 {
        self.leaf_area * self.n_leaves() as f64
    }
}

/// Place leaves. `cells` are voxel centres with the one-sided leaf area (m2)
/// each must hold; `seeds` are leaf points (may be empty) that leaves are
/// centred on where a cell has some; `cylinders` the QSM (may be empty).
pub fn insert_leaves(cells: &[(Point, f64)], voxel_size: f64, seeds: &[Point], angles: &LeafAngles, cylinders: &[Cylinder], p: &LeafParams) -> LeafMesh {
    let leaf_area = p.blade.area(p.length, p.width);
    let mut mesh = LeafMesh { leaf_area, ..Default::default() };
    if !(leaf_area > 0.0) || !(voxel_size > 0.0) {
        return mesh;
    }
    let key = |q: &Point| [(q[0] / voxel_size).floor() as i64, (q[1] / voxel_size).floor() as i64, (q[2] / voxel_size).floor() as i64];
    let mut by_cell: HashMap<[i64; 3], Vec<usize>> = HashMap::new();
    for (i, s) in seeds.iter().enumerate() {
        by_cell.entry(key(s)).or_default().push(i);
    }
    // Axis samples every 5 cm to find the nearest branch.
    let mut axis_pts: Vec<Point> = Vec::new();
    let mut axis_cyl: Vec<usize> = Vec::new();
    for (ci, c) in cylinders.iter().enumerate() {
        let n = (c.length / 0.05).ceil().max(1.0) as usize;
        for s in 0..=n {
            axis_pts.push(add(&c.start, &scale(&c.axis, c.length * s as f64 / n as f64)));
            axis_cyl.push(ci);
        }
    }
    let axis_tree = (!axis_pts.is_empty()).then(|| KdTree::new(&axis_pts));

    let mut rng = Rng::new(p.seed.max(1));
    let unit = |rng: &mut Rng| (rng.next_u64() >> 11) as f64 / (1u64 << 53) as f64;
    let task = crate::util::progress::start("placing leaves", cells.len() as u64);
    let mut carry = 0.0; // fractional leaves carried between cells so the total is met
    for (centre, area) in cells {
        task.inc(1);
        if !(*area > 0.0) {
            continue;
        }
        let want = area / leaf_area + carry;
        let n = want.floor() as usize;
        carry = want - n as f64;
        let local = by_cell.get(&key(centre));
        for _ in 0..n {
            let mut c = match local {
                Some(idx) if !idx.is_empty() => seeds[idx[rng.below(idx.len())]],
                _ => [centre[0] + (unit(&mut rng) - 0.5) * voxel_size, centre[1] + (unit(&mut rng) - 0.5) * voxel_size, centre[2] + (unit(&mut rng) - 0.5) * voxel_size],
            };
            for k in 0..3 {
                c[k] += (unit(&mut rng) - 0.5) * 2.0 * p.jitter;
            }
            let theta = angles.sample(&mut rng);
            let phi = unit(&mut rng) * TAU;
            let normal = [theta.sin() * phi.cos(), theta.sin() * phi.sin(), theta.cos()];
            // Blade direction in the leaf plane: away from the nearest branch, else random.
            let mut parent = -1i64;
            let mut away: Option<Point> = None;
            if let Some(tree) = &axis_tree {
                if let Some((j, d)) = tree.nearest(&c) {
                    if d <= p.max_branch_distance {
                        parent = axis_cyl[j] as i64;
                        away = Some(sub(&c, &axis_pts[j]));
                    }
                }
            }
            let in_plane = |v: &Point| sub(v, &scale(&normal, dot(v, &normal)));
            let mut dir = away.map(|v| in_plane(&v)).filter(|v| norm(v) > 1e-9);
            if dir.is_none() {
                let a = unit(&mut rng) * TAU;
                let helper = if normal[2].abs() < 0.9 { [0.0, 0.0, 1.0] } else { [1.0, 0.0, 0.0] };
                let u = normalize(&cross(&normal, &helper));
                let v = cross(&normal, &u);
                dir = Some(add(&scale(&u, a.cos()), &scale(&v, a.sin())));
            }
            let dir = normalize(&dir.unwrap());
            let side = cross(&normal, &dir);
            let base = sub(&c, &scale(&dir, 0.5 * p.length));
            let v0 = mesh.vertices.len() as u32;
            for b in &p.blade.vertices {
                let q = add(&scale(&dir, b[0] * p.length), &add(&scale(&side, b[1] * p.width), &scale(&normal, b[2] * p.width)));
                mesh.vertices.push(add(&base, &q));
            }
            for f in &p.blade.faces {
                mesh.faces.push([v0 + f[0], v0 + f[1], v0 + f[2]]);
            }
            mesh.centres.push(c);
            mesh.normals.push(normal);
            mesh.inclination.push(theta);
            mesh.cylinder.push(parent);
        }
    }
    mesh
}

/// Beam projection `G(theta)` of the distribution for a beam at zenith
/// `theta` (rad): the mean of `|cos angle(beam, normal)|` over the histogram
/// with a uniform azimuth (Wilson 1960 kernel).
pub fn projection(angles: &LeafAngles, beam_zenith: f64) -> f64 {
    let (st, ct) = beam_zenith.sin_cos();
    angles
        .bin_centres
        .iter()
        .zip(&angles.density)
        .map(|(&tl, &w)| {
            let (sl, cl) = tl.sin_cos();
            let a = ct * cl;
            let b = st * sl;
            let g = if b.abs() <= a.abs() || b.abs() < 1e-12 {
                a.abs()
            } else {
                let psi = (-a / b).clamp(-1.0, 1.0).acos();
                (a * (2.0 * psi / PI - 1.0) + 2.0 / PI * b * psi.sin()).abs()
            };
            w * g
        })
        .sum()
}