geop_core_math/
plain.rs

1//! Vectors in plain numbers, `[f64; N]`: for what is only ever a free
2//! choice or a picture — a section placed along a spline, a sample to draw,
3//! a point to suggest — never for a result the kernel reasons about, which
4//! is a [`crate::vector::Vector`] of scalars.
5//!
6//! Where a direction has no length, [`normalize`] gives none; what to do
7//! then is the caller's choice, made where it is called.
8
9/// `[x, y]`.
10pub type P2 = [f64; 2];
11
12/// `[x, y, z]`.
13pub type P3 = [f64; 3];
14
15pub fn add<const N: usize>(a: [f64; N], b: [f64; N]) -> [f64; N] {
16    std::array::from_fn(|i| a[i] + b[i])
17}
18
19pub fn sub<const N: usize>(a: [f64; N], b: [f64; N]) -> [f64; N] {
20    std::array::from_fn(|i| a[i] - b[i])
21}
22
23pub fn scale<const N: usize>(a: [f64; N], s: f64) -> [f64; N] {
24    a.map(|x| x * s)
25}
26
27pub fn dot<const N: usize>(a: [f64; N], b: [f64; N]) -> f64 {
28    (1..N).fold(a[0] * b[0], |sum, i| sum + a[i] * b[i])
29}
30
31/// The length of `a`, `sqrt(a . a)`.
32pub fn norm<const N: usize>(a: [f64; N]) -> f64 {
33    dot(a, a).sqrt()
34}
35
36/// How far `a` is from `b`.
37pub fn distance<const N: usize>(a: [f64; N], b: [f64; N]) -> f64 {
38    norm(sub(a, b))
39}
40
41/// `a` scaled to unit length, or `None` if it has no finite, non-zero one.
42pub fn normalize<const N: usize>(a: [f64; N]) -> Option<[f64; N]> {
43    let n = norm(a);
44    (n > 0.0 && n.is_finite()).then(|| scale(a, 1.0 / n))
45}
46
47/// The cross product `a x b`.
48pub fn cross(a: P3, b: P3) -> P3 {
49    [
50        a[1] * b[2] - a[2] * b[1],
51        a[2] * b[0] - a[0] * b[2],
52        a[0] * b[1] - a[1] * b[0],
53    ]
54}
55
56/// The 2-D cross product `a.x b.y - a.y b.x`: positive where `b` lies
57/// counter-clockwise of `a`.
58pub fn cross2(a: P2, b: P2) -> f64 {
59    a[0] * b[1] - a[1] * b[0]
60}
61
62/// Twice the signed area of the closed polygon `poly` (shoelace formula):
63/// positive where it runs counter-clockwise.
64pub fn signed_area2(poly: &[P2]) -> f64 {
65    (0..poly.len())
66        .map(|i| cross2(poly[i], poly[(i + 1) % poly.len()]))
67        .sum()
68}
69
70/// The solution `x` of `m x = r` for the 2x2 matrix `m`, by Cramer's rule:
71/// not finite where `m` is singular.
72pub fn solve2(m: [P2; 2], r: P2) -> P2 {
73    let det = m[0][0] * m[1][1] - m[0][1] * m[1][0];
74    [
75        (m[1][1] * r[0] - m[0][1] * r[1]) / det,
76        (m[0][0] * r[1] - m[1][0] * r[0]) / det,
77    ]
78}
79
80/// The inverse of the 2x2 matrix `m`: not finite where it is singular.
81pub fn inverse2(m: [P2; 2]) -> [P2; 2] {
82    let det = m[0][0] * m[1][1] - m[0][1] * m[1][0];
83    [
84        [m[1][1] / det, -m[0][1] / det],
85        [-m[1][0] / det, m[0][0] / det],
86    ]
87}
88
89/// The angle `a`, turned by whole turns to within half a turn of
90/// `reference`: in `[reference - π, reference + π)`, how an angle measured
91/// in `(-π, π]` is followed on from the one before.
92pub fn unwrap_angle(a: f64, reference: f64) -> f64 {
93    use std::f64::consts::{PI, TAU};
94    reference + (a - reference + PI).rem_euclid(TAU) - PI
95}
96
97/// The length of the diagonal of the box from `lo` to `hi`: how far apart
98/// any two points in it can be.
99pub fn diagonal<const N: usize>(lo: [f64; N], hi: [f64; N]) -> f64 {
100    (0..N).map(|k| (hi[k] - lo[k]).powi(2)).sum::<f64>().sqrt()
101}
102
103/// The box around `points`: its lowest corner and its highest, or `None`
104/// if there are no points.
105pub fn bounds<const N: usize>(
106    points: impl IntoIterator<Item = [f64; N]>,
107) -> Option<([f64; N], [f64; N])> {
108    let mut points = points.into_iter();
109    let first = points.next()?;
110    Some(points.fold((first, first), |(lo, hi), p| {
111        (
112            std::array::from_fn(|k| lo[k].min(p[k])),
113            std::array::from_fn(|k| hi[k].max(p[k])),
114        )
115    }))
116}
117
118/// The mean of `points`, summed in their order and then divided by their
119/// count, or `None` if there are no points.
120pub fn centroid<const N: usize>(points: impl IntoIterator<Item = [f64; N]>) -> Option<[f64; N]> {
121    let mut sum = [0.0; N];
122    let mut n = 0usize;
123    for p in points {
124        for k in 0..N {
125            sum[k] += p[k];
126        }
127        n += 1;
128    }
129    (n > 0).then(|| sum.map(|s| s / n as f64))
130}
geop_core_math/plain.rs

geop_core_math/
plain.rs
