Add calculation of derivatives of 2D curve data.

This can be used to plot curve data and analyse how the curves are
moving, so we can detect jittery values.

GitHub issue #268

lib/rust/mmscenegraph/src/math/curve_analysis.rs

@@ -0,0 +1,116 @@
+//
+// Copyright (C) 2024 David Cattermole.
+//
+// This file is part of mmSolver.
+//
+// mmSolver is free software: you can redistribute it and/or modify it
+// under the terms of the GNU Lesser General Public License as
+// published by the Free Software Foundation, either version 3 of the
+// License, or (at your option) any later version.
+//
+// mmSolver is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with mmSolver.  If not, see <https://www.gnu.org/licenses/>.
+// ====================================================================
+//
+
+use anyhow::bail;
+use anyhow::Result;
+use log::debug;
+
+use crate::constant::Real;
+
+/// Given arrays of time and value points, calculate the derivative using
+/// previous and next values (except for the first and last points).
+/// Writes results into the provided output slice.
+///
+/// # Arguments
+/// * `times` - Slice of time points
+/// * `values` - Slice of corresponding values
+/// * `out` - Slice where derivatives will be written, must be same length as inputs
+///
+/// # Returns
+/// `Ok(())` if successful, `Err` if slice lengths don't match
+///
+/// This function avoids allocations and is designed to be easy for
+/// compilers to auto-vectorize.
+fn calc_derivative(
+    times: &[Real],
+    values: &[Real],
+    out: &mut [Real],
+) -> Result<()> {
+    // Verify slice lengths match
+    if times.len() != values.len() || times.len() != out.len() {
+        bail!("Input and output slices must have equal length");
+    }
+
+    let n = times.len();
+    if n < 2 {
+        // Zero out the output for small inputs, because only a single
+        // time/value cannot change, so there is no derivative.
+        out.fill(0.0);
+
+        return Ok(());
+    }
+
+    // First point, using forward difference: (f(x + h) - f(x)) / h
+    out[0] = (values[1] - values[0]) / (times[1] - times[0]);
+
+    // Middle points, using central difference: (f(x + h) - f(x - h)) / (2h)
+    for i in 1..n - 1 {
+        // Forward and backward time differences
+        let dt_forward = times[i + 1] - times[i];
+        let dt_backward = times[i] - times[i - 1];
+        let total_dt = dt_forward + dt_backward;
+
+        // Weights for weighted average
+        let w_backward = dt_forward / total_dt;
+        let w_forward = dt_backward / total_dt;
+
+        // Forward and backward value differences
+        let dv_forward = values[i + 1] - values[i];
+        let dv_backward = values[i] - values[i - 1];
+
+        // Weighted average of forward and backward derivatives
+        out[i] = w_backward * (dv_backward / dt_backward)
+            + w_forward * (dv_forward / dt_forward);
+    }
+
+    // Last point, using backward difference: (f(x) - f(x - h)) / h
+    let last = n - 1;
+    out[last] =
+        (values[last] - values[last - 1]) / (times[last] - times[last - 1]);
+
+    Ok(())
+}
+
+/// Calculate derivatives (velocity, acceleration, jerk) from time and
+/// height data.
+///
+/// Returns vectors of the same length as input by using forward,
+/// central, and backward differences
+pub fn calculate_derivatives(
+    times: &[Real],
+    heights: &[Real],
+) -> Result<(Vec<Real>, Vec<Real>, Vec<Real>)> {
+    let n = heights.len();
+    if n < 4 {
+        bail!("Need at least 4 points for derivative analysis");
+    }
+
+    // TODO: Should these vectors be given to this function to be
+    // allocated?
+    let mut velocity = vec![0.0; n];
+    let mut acceleration = vec![0.0; n];
+    let mut jerk = vec![0.0; n];
+
+    calc_derivative(times, heights, &mut velocity)?;
+    calc_derivative(times, &velocity, &mut acceleration)?;
+    calc_derivative(times, &acceleration, &mut jerk)?;
+
+    Ok((velocity, acceleration, jerk))
+}

lib/rust/mmscenegraph/src/math/mod.rs

@@ -19,6 +19,7 @@
 //
 
 pub mod camera;
+pub mod curve_analysis;
 pub mod curve_fit;
 pub mod dag;
 pub mod line;

lib/rust/mmscenegraph/tests/curve_analysis_derivatives.rs

@@ -0,0 +1,188 @@
+//
+// Copyright (C) 2024 David Cattermole.
+//
+// This file is part of mmSolver.
+//
+// mmSolver is free software: you can redistribute it and/or modify it
+// under the terms of the GNU Lesser General Public License as
+// published by the Free Software Foundation, either version 3 of the
+// License, or (at your option) any later version.
+//
+// mmSolver is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with mmSolver.  If not, see <https://www.gnu.org/licenses/>.
+// ====================================================================
+//
+
+mod common;
+
+use anyhow::Result;
+use approx::assert_relative_eq;
+
+use mmscenegraph_rust::math::curve_analysis::calculate_derivatives;
+
+fn print_arrays(velocity: &[f64], acceleration: &[f64], jerk: &[f64]) {
+    println!("Found {} velocity:", velocity.len());
+    for (i, v) in velocity.iter().enumerate() {
+        println!("i={i} v={v}");
+    }
+
+    println!("Found {} acceleration:", acceleration.len());
+    for (i, v) in acceleration.iter().enumerate() {
+        println!("i={i} v={v}");
+    }
+
+    println!("Found {} jerk:", jerk.len());
+    for (i, v) in jerk.iter().enumerate() {
+        println!("i={i} v={v}");
+    }
+}
+
+#[test]
+fn calculate_derivatives1() -> Result<()> {
+    // Constant speed, upwards.
+    let times = vec![0.0, 1.0, 2.0, 3.0, 4.0, 5.0];
+    let heights = vec![1.0, 1.1, 1.2, 1.3, 1.4, 1.5];
+
+    let (velocity, acceleration, jerk) =
+        calculate_derivatives(&times, &heights)?;
+
+    print_arrays(&velocity, &acceleration, &jerk);
+
+    assert_eq!(velocity.len(), 6);
+    assert_eq!(acceleration.len(), 6);
+    assert_eq!(jerk.len(), 6);
+
+    assert_relative_eq!(velocity[0], 0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[1], 0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[2], 0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[3], 0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[4], 0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[5], 0.1, epsilon = 1.0e-9);
+
+    assert_relative_eq!(acceleration[0], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[1], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[2], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[3], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[4], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[5], 0.0, epsilon = 1.0e-9);
+
+    assert_relative_eq!(jerk[0], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[1], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[2], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[3], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[4], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[5], 0.0, epsilon = 1.0e-9);
+
+    Ok(())
+}
+
+#[test]
+fn calculate_derivatives2() -> Result<()> {
+    // Constant velocity, downwards.
+    let times = vec![0.0, 1.0, 2.0, 3.0, 4.0, 5.0];
+    let heights = vec![-1.0, -1.1, -1.2, -1.3, -1.4, -1.5];
+
+    let (velocity, acceleration, jerk) =
+        calculate_derivatives(&times, &heights)?;
+
+    print_arrays(&velocity, &acceleration, &jerk);
+
+    assert_eq!(velocity.len(), 6);
+    assert_eq!(acceleration.len(), 6);
+    assert_eq!(jerk.len(), 6);
+
+    assert_relative_eq!(velocity[0], -0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[1], -0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[2], -0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[3], -0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[4], -0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[5], -0.1, epsilon = 1.0e-9);
+
+    assert_relative_eq!(acceleration[0], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[1], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[2], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[3], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[4], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[5], 0.0, epsilon = 1.0e-9);
+
+    assert_relative_eq!(jerk[0], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[1], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[2], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[3], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[4], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[5], 0.0, epsilon = 1.0e-9);
+
+    Ok(())
+}
+
+#[test]
+fn calculate_derivatives3() -> Result<()> {
+    // Variable velocity.
+    let times = vec![0.0, 1.0, 2.0, 3.0, 4.0, 5.0];
+    let heights = vec![1.0, 1.1, 1.25, 1.5, 2.0, 4.0];
+
+    let (velocity, acceleration, jerk) =
+        calculate_derivatives(&times, &heights)?;
+
+    print_arrays(&velocity, &acceleration, &jerk);
+
+    assert_eq!(velocity.len(), 6);
+    assert_eq!(acceleration.len(), 6);
+    assert_eq!(jerk.len(), 6);
+
+    assert_relative_eq!(velocity[0], 0.1, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[1], 0.125, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[2], 0.2, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[3], 0.375, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[4], 1.25, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[5], 2.0, epsilon = 1.0e-9);
+
+    Ok(())
+}
+
+#[test]
+fn calculate_derivatives4() -> Result<()> {
+    let times = vec![0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0];
+    let heights = vec![0.0, 1.0, 4.0, 9.0, 16.0, 25.0, 36.0]; // f(x) = x^2
+
+    let (velocity, acceleration, jerk) =
+        calculate_derivatives(&times, &heights)?;
+
+    print_arrays(&velocity, &acceleration, &jerk);
+
+    assert_eq!(velocity.len(), 7);
+    assert_eq!(acceleration.len(), 7);
+    assert_eq!(jerk.len(), 7);
+
+    // Expected derivatives of x^2;
+    assert_relative_eq!(velocity[0], 1.0, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[1], 2.0, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[2], 4.0, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[3], 6.0, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[4], 8.0, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[5], 10.0, epsilon = 1.0e-9);
+    assert_relative_eq!(velocity[6], 11.0, epsilon = 1.0e-9);
+
+    assert_relative_eq!(acceleration[0], 1.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[1], 1.5, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[2], 2.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[3], 2.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[4], 2.0, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[5], 1.5, epsilon = 1.0e-9);
+    assert_relative_eq!(acceleration[6], 1.0, epsilon = 1.0e-9);
+
+    assert_relative_eq!(jerk[0], 0.5, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[1], 0.5, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[2], 0.25, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[3], 0.0, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[4], -0.25, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[5], -0.5, epsilon = 1.0e-9);
+    assert_relative_eq!(jerk[6], -0.5, epsilon = 1.0e-9);
+
+    Ok(())
+}