253 lines
6.8 KiB
Rust
253 lines
6.8 KiB
Rust
//! Common utility functions for the WiFi-DensePose system.
|
|
//!
|
|
//! This module provides helper functions used throughout the crate.
|
|
|
|
use ndarray::{Array1, Array2};
|
|
use num_complex::Complex64;
|
|
|
|
/// Computes the magnitude (absolute value) of complex numbers.
|
|
#[must_use]
|
|
pub fn complex_magnitude(data: &Array2<Complex64>) -> Array2<f64> {
|
|
data.mapv(num_complex::Complex::norm)
|
|
}
|
|
|
|
/// Computes the phase (argument) of complex numbers in radians.
|
|
#[must_use]
|
|
pub fn complex_phase(data: &Array2<Complex64>) -> Array2<f64> {
|
|
data.mapv(num_complex::Complex::arg)
|
|
}
|
|
|
|
/// Unwraps phase values to remove discontinuities.
|
|
///
|
|
/// Phase unwrapping corrects for the 2*pi jumps that occur when phase
|
|
/// values wrap around from pi to -pi.
|
|
#[must_use]
|
|
pub fn unwrap_phase(phase: &Array1<f64>) -> Array1<f64> {
|
|
let mut unwrapped = phase.clone();
|
|
let pi = std::f64::consts::PI;
|
|
let two_pi = 2.0 * pi;
|
|
|
|
for i in 1..unwrapped.len() {
|
|
let diff = unwrapped[i] - unwrapped[i - 1];
|
|
if diff > pi {
|
|
for j in i..unwrapped.len() {
|
|
unwrapped[j] -= two_pi;
|
|
}
|
|
} else if diff < -pi {
|
|
for j in i..unwrapped.len() {
|
|
unwrapped[j] += two_pi;
|
|
}
|
|
}
|
|
}
|
|
|
|
unwrapped
|
|
}
|
|
|
|
/// Normalizes values to the range [0, 1].
|
|
#[must_use]
|
|
pub fn normalize_min_max(data: &Array1<f64>) -> Array1<f64> {
|
|
let min = data.iter().copied().fold(f64::INFINITY, f64::min);
|
|
let max = data.iter().copied().fold(f64::NEG_INFINITY, f64::max);
|
|
|
|
if (max - min).abs() < f64::EPSILON {
|
|
return Array1::zeros(data.len());
|
|
}
|
|
|
|
data.mapv(|x| (x - min) / (max - min))
|
|
}
|
|
|
|
/// Normalizes values using z-score normalization.
|
|
#[must_use]
|
|
pub fn normalize_zscore(data: &Array1<f64>) -> Array1<f64> {
|
|
let mean = data.mean().unwrap_or(0.0);
|
|
let std = data.std(0.0);
|
|
|
|
if std.abs() < f64::EPSILON {
|
|
return Array1::zeros(data.len());
|
|
}
|
|
|
|
data.mapv(|x| (x - mean) / std)
|
|
}
|
|
|
|
/// Calculates the Signal-to-Noise Ratio in dB.
|
|
#[must_use]
|
|
#[allow(clippy::cast_precision_loss)]
|
|
pub fn calculate_snr_db(signal: &Array1<f64>, noise: &Array1<f64>) -> f64 {
|
|
let signal_power: f64 = signal.iter().map(|x| x * x).sum::<f64>() / signal.len() as f64;
|
|
let noise_power: f64 = noise.iter().map(|x| x * x).sum::<f64>() / noise.len() as f64;
|
|
|
|
if noise_power.abs() < f64::EPSILON {
|
|
return f64::INFINITY;
|
|
}
|
|
|
|
10.0 * (signal_power / noise_power).log10()
|
|
}
|
|
|
|
/// Applies a moving average filter.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Panics if the data array is not contiguous in memory.
|
|
#[must_use]
|
|
#[allow(clippy::cast_precision_loss)]
|
|
pub fn moving_average(data: &Array1<f64>, window_size: usize) -> Array1<f64> {
|
|
if window_size == 0 || window_size > data.len() {
|
|
return data.clone();
|
|
}
|
|
|
|
let mut result = Array1::zeros(data.len());
|
|
let half_window = window_size / 2;
|
|
|
|
// ndarray Array1 is always contiguous, but handle gracefully if not
|
|
let Some(slice) = data.as_slice() else {
|
|
return data.clone();
|
|
};
|
|
|
|
for i in 0..data.len() {
|
|
let start = i.saturating_sub(half_window);
|
|
let end = (i + half_window + 1).min(data.len());
|
|
let window = &slice[start..end];
|
|
result[i] = window.iter().sum::<f64>() / window.len() as f64;
|
|
}
|
|
|
|
result
|
|
}
|
|
|
|
/// Clamps a value to a range.
|
|
#[must_use]
|
|
pub fn clamp<T: PartialOrd>(value: T, min: T, max: T) -> T {
|
|
if value < min {
|
|
min
|
|
} else if value > max {
|
|
max
|
|
} else {
|
|
value
|
|
}
|
|
}
|
|
|
|
/// Linearly interpolates between two values.
|
|
#[must_use]
|
|
pub fn lerp(a: f64, b: f64, t: f64) -> f64 {
|
|
(b - a).mul_add(t, a)
|
|
}
|
|
|
|
/// Converts degrees to radians.
|
|
#[must_use]
|
|
pub fn deg_to_rad(degrees: f64) -> f64 {
|
|
degrees.to_radians()
|
|
}
|
|
|
|
/// Converts radians to degrees.
|
|
#[must_use]
|
|
pub fn rad_to_deg(radians: f64) -> f64 {
|
|
radians.to_degrees()
|
|
}
|
|
|
|
/// Calculates the Euclidean distance between two points.
|
|
#[must_use]
|
|
pub fn euclidean_distance(p1: (f64, f64), p2: (f64, f64)) -> f64 {
|
|
let dx = p2.0 - p1.0;
|
|
let dy = p2.1 - p1.1;
|
|
dx.hypot(dy)
|
|
}
|
|
|
|
/// Calculates the Euclidean distance in 3D.
|
|
#[must_use]
|
|
pub fn euclidean_distance_3d(p1: (f64, f64, f64), p2: (f64, f64, f64)) -> f64 {
|
|
let dx = p2.0 - p1.0;
|
|
let dy = p2.1 - p1.1;
|
|
let dz = p2.2 - p1.2;
|
|
(dx.mul_add(dx, dy.mul_add(dy, dz * dz))).sqrt()
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
use ndarray::array;
|
|
|
|
#[test]
|
|
fn test_normalize_min_max() {
|
|
let data = array![0.0, 5.0, 10.0];
|
|
let normalized = normalize_min_max(&data);
|
|
|
|
assert!((normalized[0] - 0.0).abs() < 1e-10);
|
|
assert!((normalized[1] - 0.5).abs() < 1e-10);
|
|
assert!((normalized[2] - 1.0).abs() < 1e-10);
|
|
}
|
|
|
|
#[test]
|
|
fn test_normalize_zscore() {
|
|
let data = array![1.0, 2.0, 3.0, 4.0, 5.0];
|
|
let normalized = normalize_zscore(&data);
|
|
|
|
// Mean should be approximately 0
|
|
assert!(normalized.mean().unwrap().abs() < 1e-10);
|
|
}
|
|
|
|
#[test]
|
|
fn test_moving_average() {
|
|
let data = array![1.0, 2.0, 3.0, 4.0, 5.0];
|
|
let smoothed = moving_average(&data, 3);
|
|
|
|
// Middle value should be average of 2, 3, 4
|
|
assert!((smoothed[2] - 3.0).abs() < 1e-10);
|
|
}
|
|
|
|
#[test]
|
|
fn test_clamp() {
|
|
assert_eq!(clamp(5, 0, 10), 5);
|
|
assert_eq!(clamp(-5, 0, 10), 0);
|
|
assert_eq!(clamp(15, 0, 10), 10);
|
|
}
|
|
|
|
#[test]
|
|
fn test_lerp() {
|
|
assert!((lerp(0.0, 10.0, 0.5) - 5.0).abs() < 1e-10);
|
|
assert!((lerp(0.0, 10.0, 0.0) - 0.0).abs() < 1e-10);
|
|
assert!((lerp(0.0, 10.0, 1.0) - 10.0).abs() < 1e-10);
|
|
}
|
|
|
|
#[test]
|
|
fn test_deg_rad_conversion() {
|
|
let degrees = 180.0;
|
|
let radians = deg_to_rad(degrees);
|
|
assert!((radians - std::f64::consts::PI).abs() < 1e-10);
|
|
|
|
let back = rad_to_deg(radians);
|
|
assert!((back - degrees).abs() < 1e-10);
|
|
}
|
|
|
|
#[test]
|
|
fn test_euclidean_distance() {
|
|
let dist = euclidean_distance((0.0, 0.0), (3.0, 4.0));
|
|
assert!((dist - 5.0).abs() < 1e-10);
|
|
}
|
|
|
|
#[test]
|
|
fn test_unwrap_phase() {
|
|
let pi = std::f64::consts::PI;
|
|
// Simulate a phase wrap
|
|
let phase = array![0.0, pi / 2.0, pi, -pi + 0.1, -pi / 2.0];
|
|
let unwrapped = unwrap_phase(&phase);
|
|
|
|
// After unwrapping, the phase should be monotonically increasing
|
|
for i in 1..unwrapped.len() {
|
|
// Allow some tolerance for the discontinuity correction
|
|
assert!(
|
|
unwrapped[i] >= unwrapped[i - 1] - 0.5,
|
|
"Phase should be mostly increasing after unwrapping"
|
|
);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_snr_calculation() {
|
|
let signal = array![1.0, 1.0, 1.0, 1.0];
|
|
let noise = array![0.1, 0.1, 0.1, 0.1];
|
|
|
|
let snr = calculate_snr_db(&signal, &noise);
|
|
// SNR should be 20 dB (10 * log10(1/0.01) = 10 * log10(100) = 20)
|
|
assert!((snr - 20.0).abs() < 1e-10);
|
|
}
|
|
}
|