nautilus_core/
math.rs

Help
1// -------------------------------------------------------------------------------------------------
2//  Copyright (C) 2015-2025 Nautech Systems Pty Ltd. All rights reserved.
3//  https://nautechsystems.io
4//
5//  Licensed under the GNU Lesser General Public License Version 3.0 (the "License");
6//  You may not use this file except in compliance with the License.
7//  You may obtain a copy of the License at https://www.gnu.org/licenses/lgpl-3.0.en.html
8//
9//  Unless required by applicable law or agreed to in writing, software
10//  distributed under the License is distributed on an "AS IS" BASIS,
11//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12//  See the License for the specific language governing permissions and
13//  limitations under the License.
14// -------------------------------------------------------------------------------------------------
15
16#[inline]
17#[must_use]
18pub fn linear_weight(x1: f64, x2: f64, x: f64) -> f64 {
19    (x - x1) / (x2 - x1)
20}
21
22#[inline]
23#[must_use]
24pub fn linear_weighting(y1: f64, y2: f64, x1_diff: f64) -> f64 {
25    x1_diff.mul_add(y2 - y1, y1)
26}
27
28#[inline]
29#[must_use]
30pub fn pos_search(x: f64, xs: &[f64]) -> usize {
31    let n_elem = xs.len();
32    let pos = xs.partition_point(|&val| val < x);
33    std::cmp::min(std::cmp::max(pos.saturating_sub(1), 0), n_elem - 1)
34}
35
36#[inline]
37#[must_use]
38pub fn quad_polynomial(x: f64, x0: f64, x1: f64, x2: f64, y0: f64, y1: f64, y2: f64) -> f64 {
39    y0 * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2))
40        + y1 * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2))
41        + y2 * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1))
42}
43
44#[must_use]
45pub fn quadratic_interpolation(x: f64, xs: &[f64], ys: &[f64]) -> f64 {
46    let n_elem = xs.len();
47    let epsilon = 1e-8;
48
49    assert!(
50        (n_elem >= 3),
51        "Need at least 3 points for quadratic interpolation"
52    );
53
54    if x <= xs[0] {
55        return ys[0];
56    }
57
58    if x >= xs[n_elem - 1] {
59        return ys[n_elem - 1];
60    }
61
62    let pos = pos_search(x, xs);
63
64    if (xs[pos] - x).abs() < epsilon {
65        return ys[pos];
66    }
67
68    if pos == 0 {
69        return quad_polynomial(x, xs[0], xs[1], xs[2], ys[0], ys[1], ys[2]);
70    }
71
72    if pos == n_elem - 2 {
73        return quad_polynomial(
74            x,
75            xs[n_elem - 3],
76            xs[n_elem - 2],
77            xs[n_elem - 1],
78            ys[n_elem - 3],
79            ys[n_elem - 2],
80            ys[n_elem - 1],
81        );
82    }
83
84    let w = linear_weight(xs[pos], xs[pos + 1], x);
85
86    linear_weighting(
87        quad_polynomial(
88            x,
89            xs[pos - 1],
90            xs[pos],
91            xs[pos + 1],
92            ys[pos - 1],
93            ys[pos],
94            ys[pos + 1],
95        ),
96        quad_polynomial(
97            x,
98            xs[pos],
99            xs[pos + 1],
100            xs[pos + 2],
101            ys[pos],
102            ys[pos + 1],
103            ys[pos + 2],
104        ),
105        w,
106    )
107}
nautilus_core/math.rs

nautilus_core/
math.rs
