pub fn compute_trace(matrix: &Array2<f64>) -> f64
Compute trace of a matrix (sum of diagonal elements)