Add calculation of derivatives of 2D curve data.

david-cattermole · david-cattermole · commit b3474a06ee7d · 2024-10-28T02:19:10.000+11:00
This can be used to plot curve data and analyse how the curves are moving, so we can detect jittery values. GitHub issue #268
diff --git a/lib/rust/mmscenegraph/src/math/curve_analysis.rs b/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))
+}
diff --git a/lib/rust/mmscenegraph/src/math/mod.rs b/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;
diff --git a/lib/rust/mmscenegraph/tests/curve_analysis_derivatives.rs b/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(())
+}
