Curve analysis derivatives - give memory to function

We preallocate memory then pass it to functions to compute the
derivatives directly.

We do this because it's usually better practice not to allocate memory
often and instead offer control over the user where the memory comes
from.

Author: david-cattermole

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

@@ -88,29 +88,128 @@ fn calc_derivative(
     Ok(())
 }
 
-/// Calculate derivatives (velocity, acceleration, jerk) from time and
-/// height data.
+/// Allocate vectors for 1st order derivatives.
+pub fn allocate_derivatives_order_1(num: usize) -> Result<Vec<Real>> {
+    let velocity = vec![0.0; num];
+    Ok(velocity)
+}
+
+/// Allocate vectors for 2nd order derivatives.
+pub fn allocate_derivatives_order_2(
+    num: usize,
+) -> Result<(Vec<Real>, Vec<Real>)> {
+    let velocity = vec![0.0; num];
+    let acceleration = vec![0.0; num];
+    Ok((velocity, acceleration))
+}
+
+/// Allocate vectors for 3rd order derivatives.
+pub fn allocate_derivatives_order_3(
+    num: usize,
+) -> Result<(Vec<Real>, Vec<Real>, Vec<Real>)> {
+    let velocity = vec![0.0; num];
+    let acceleration = vec![0.0; num];
+    let jerk = vec![0.0; num];
+    Ok((velocity, acceleration, jerk))
+}
+
+/// Calculate 1st order derivatives (velocity) from time and value
+/// data.
 ///
-/// Returns vectors of the same length as input by using forward,
-/// central, and backward differences
-pub fn calculate_derivatives(
+/// Returns vectors of the same length as input by using finite
+/// differences.
+pub fn calculate_derivatives_order_1(
     times: &[Real],
-    heights: &[Real],
-) -> Result<(Vec<Real>, Vec<Real>, Vec<Real>)> {
-    let n = heights.len();
+    values: &[Real],
+    out_velocity: &mut [Real],
+) -> Result<()> {
+    // Verify slice lengths match
+    if times.len() != values.len() || times.len() != out_velocity.len() {
+        bail!("Input and output slices must have equal length; times.len()={} values.len()={} out_velocity.len()={}",
+              times.len(),
+              values.len(),
+              out_velocity.len())
+    }
+
+    let n = values.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, values, out_velocity)?;
 
-    calc_derivative(times, heights, &mut velocity)?;
-    calc_derivative(times, &velocity, &mut acceleration)?;
-    calc_derivative(times, &acceleration, &mut jerk)?;
+    Ok(())
+}
 
-    Ok((velocity, acceleration, jerk))
+/// Calculate 2nd order derivatives (velocity and acceleration) from
+/// time and value data.
+///
+/// Returns vectors of the same length as input by using finite
+/// differences.
+pub fn calculate_derivatives_order_2(
+    times: &[Real],
+    values: &[Real],
+    out_velocity: &mut [Real],
+    out_acceleration: &mut [Real],
+) -> Result<()> {
+    // Verify slice lengths match
+    if times.len() != values.len()
+        || times.len() != out_velocity.len()
+        || times.len() != out_acceleration.len()
+    {
+        bail!("Input and output slices must have equal length; times.len()={} values.len()={} out_velocity.len()={} out_acceleration.len()={}",
+        times.len(),
+        values.len(),
+        out_velocity.len(),
+        out_acceleration.len(),
+        )
+    }
+
+    let n = values.len();
+    if n < 4 {
+        bail!("Need at least 4 points for derivative analysis");
+    }
+
+    calc_derivative(times, values, out_velocity)?;
+    calc_derivative(times, &out_velocity, out_acceleration)?;
+
+    Ok(())
+}
+
+/// Calculate 3rd order derivatives (velocity, acceleration, and jerk)
+/// from time and value data.
+///
+/// Returns vectors of the same length as input by using finite
+/// differences.
+pub fn calculate_derivatives_order_3(
+    times: &[Real],
+    values: &[Real],
+    out_velocity: &mut [Real],
+    out_acceleration: &mut [Real],
+    out_jerk: &mut [Real],
+) -> Result<()> {
+    // Verify slice lengths match
+    if times.len() != values.len()
+        || times.len() != out_velocity.len()
+        || times.len() != out_acceleration.len()
+        || times.len() != out_jerk.len()
+    {
+        bail!("Input and output slices must have equal length; times.len()={} values.len()={} out_velocity.len()={} out_acceleration.len()={} out_jerk.len()={}",
+              times.len(),
+              values.len(),
+              out_velocity.len(),
+              out_acceleration.len(),
+              out_jerk.len());
+    }
+
+    let n = values.len();
+    if n < 4 {
+        bail!("Need at least 4 points for derivative analysis");
+    }
+
+    calc_derivative(times, values, out_velocity)?;
+    calc_derivative(times, out_velocity, out_acceleration)?;
+    calc_derivative(times, out_acceleration, out_jerk)?;
+
+    Ok(())
 }

lib/rust/mmscenegraph/tests/curve_analysis_derivatives.rs

@@ -23,20 +23,24 @@ mod common;
 use anyhow::Result;
 use approx::assert_relative_eq;
 
-use mmscenegraph_rust::math::curve_analysis::calculate_derivatives;
+use mmscenegraph_rust::math::curve_analysis::allocate_derivatives_order_3;
+use mmscenegraph_rust::math::curve_analysis::calculate_derivatives_order_3;
 
 fn print_arrays(velocity: &[f64], acceleration: &[f64], jerk: &[f64]) {
-    println!("Found {} velocity:", velocity.len());
+    println!("Velocity (1st order derivative) count {} :", velocity.len());
     for (i, v) in velocity.iter().enumerate() {
         println!("i={i} v={v}");
     }
 
-    println!("Found {} acceleration:", acceleration.len());
+    println!(
+        "Acceleration (2nd order derivative) count {} :",
+        acceleration.len()
+    );
     for (i, v) in acceleration.iter().enumerate() {
         println!("i={i} v={v}");
     }
 
-    println!("Found {} jerk:", jerk.len());
+    println!("Jerk (3rd order derivative) count {} :", jerk.len());
     for (i, v) in jerk.iter().enumerate() {
         println!("i={i} v={v}");
     }
@@ -48,8 +52,18 @@ fn calculate_derivatives1() -> Result<()> {
     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)?;
+    // let (velocity, acceleration, jerk) =
+    //     calculate_derivatives(&times, &heights)?;
+
+    let (mut velocity, mut acceleration, mut jerk) =
+        allocate_derivatives_order_3(times.len())?;
+    calculate_derivatives_order_3(
+        &times,
+        &heights,
+        &mut velocity,
+        &mut acceleration,
+        &mut jerk,
+    )?;
 
     print_arrays(&velocity, &acceleration, &jerk);
 
@@ -87,8 +101,18 @@ fn calculate_derivatives2() -> Result<()> {
     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)?;
+    // let (velocity, acceleration, jerk) =
+    //     calculate_derivatives(&times, &heights)?;
+
+    let (mut velocity, mut acceleration, mut jerk) =
+        allocate_derivatives_order_3(times.len())?;
+    calculate_derivatives_order_3(
+        &times,
+        &heights,
+        &mut velocity,
+        &mut acceleration,
+        &mut jerk,
+    )?;
 
     print_arrays(&velocity, &acceleration, &jerk);
 
@@ -126,8 +150,15 @@ fn calculate_derivatives3() -> Result<()> {
     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)?;
+    let (mut velocity, mut acceleration, mut jerk) =
+        allocate_derivatives_order_3(times.len())?;
+    calculate_derivatives_order_3(
+        &times,
+        &heights,
+        &mut velocity,
+        &mut acceleration,
+        &mut jerk,
+    )?;
 
     print_arrays(&velocity, &acceleration, &jerk);
 
@@ -150,8 +181,15 @@ 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)?;
+    let (mut velocity, mut acceleration, mut jerk) =
+        allocate_derivatives_order_3(times.len())?;
+    calculate_derivatives_order_3(
+        &times,
+        &heights,
+        &mut velocity,
+        &mut acceleration,
+        &mut jerk,
+    )?;
 
     print_arrays(&velocity, &acceleration, &jerk);