scirs2_linalg/

lapack.rs

//! LAPACK (Linear Algebra Package) interface
//!
//! This module provides interfaces to LAPACK functions.
//!
//! LAPACK (Linear Algebra Package) provides routines for solving systems of
//! linear equations, least-squares solutions of linear systems, eigenvalue
//! problems, and singular value decomposition.

use crate::error::{LinalgError, LinalgResult};
use scirs2_core::ndarray::{array, Array1, Array2, ArrayView2, ScalarOperand};
use scirs2_core::numeric::{Float, NumAssign};
use std::iter::Sum;

/// LU decomposition structure
pub struct LUDecomposition<F: Float> {
    /// LU decomposition result (combined L and U matrices)
    pub lu: Array2<F>,
    /// Permutation indices
    pub piv: Vec<usize>,
    /// Permutation sign (+1 or -1)
    pub sign: F,
}

/// QR decomposition structure
pub struct QRDecomposition<F: Float> {
    /// Q matrix (orthogonal)
    pub q: Array2<F>,
    /// R matrix (upper triangular)
    pub r: Array2<F>,
}

/// SVD decomposition structure
pub struct SVDDecomposition<F: Float> {
    /// U matrix (left singular vectors)
    pub u: Array2<F>,
    /// Singular values
    pub s: Array1<F>,
    /// V^T matrix (right singular vectors)
    pub vt: Array2<F>,
}

/// Eigenvalue decomposition structure
pub struct EigDecomposition<F: Float> {
    /// Eigenvalues
    pub eigenvalues: Array1<F>,
    /// Eigenvectors (column-wise)
    pub eigenvectors: Array2<F>,
}

/// Performs LU decomposition with partial pivoting.
///
/// # Arguments
///
/// * `a` - Input matrix
///
/// # Returns
///
/// * LU decomposition result
///
/// # Examples
///
/// ```
/// use scirs2_core::ndarray::{array, ScalarOperand};
/// use scirs2_linalg::lapack::lu_factor;
///
/// let a = array![[2.0, 1.0, 1.0], [4.0, 3.0, 3.0], [8.0, 7.0, 9.0]];
/// let lu_result = lu_factor(&a.view()).unwrap();
///
/// // Check that P*A = L*U
/// // (implementation dependent, so not shown here)
/// ```
#[allow(dead_code)]
pub fn lu_factor<F>(a: &ArrayView2<F>) -> LinalgResult<LUDecomposition<F>>
where
    F: Float + NumAssign,
{
    let n = a.nrows();
    let m = a.ncols();

    if n == 0 || m == 0 {
        return Err(LinalgError::ComputationError(
            "Empty matrix provided".to_string(),
        ));
    }

    // Create a copy of the input matrix that we'll update in-place
    let mut lu = a.to_owned();

    // Initialize permutation vector
    let mut piv = (0..n).collect::<Vec<usize>>();
    let mut sign = F::one(); // Keeps track of the permutation sign

    // Gaussian elimination with partial pivoting
    for k in 0..n.min(m) {
        // Find pivot
        let mut p = k;
        let mut max_val = lu[[k, k]].abs();

        for i in k + 1..n {
            let abs_val = lu[[i, k]].abs();
            if abs_val > max_val {
                max_val = abs_val;
                p = i;
            }
        }

        // Check for singularity
        if max_val < F::epsilon() {
            // Calculate condition number estimate based on pivot ratio
            let condition_estimate = if max_val > F::zero() {
                Some((F::one() / max_val).to_f64().unwrap_or(1e16))
            } else {
                None
            };

            return Err(LinalgError::singularmatrix_with_suggestions(
                "LU decomposition",
                (n, m),
                condition_estimate,
            ));
        }

        // Swap rows if necessary
        if p != k {
            for j in 0..m {
                let temp = lu[[k, j]];
                lu[[k, j]] = lu[[p, j]];
                lu[[p, j]] = temp;
            }

            piv.swap(k, p);

            // Update permutation sign
            sign = -sign;
        }

        // Compute multipliers and eliminate k-th column
        for i in (k + 1)..n {
            lu[[i, k]] = lu[[i, k]] / lu[[k, k]];

            for j in k + 1..m {
                lu[[i, j]] = lu[[i, j]] - lu[[i, k]] * lu[[k, j]];
            }
        }
    }

    Ok(LUDecomposition { lu, piv, sign })
}

/// Performs QR decomposition using Householder reflections.
///
/// # Arguments
///
/// * `a` - Input matrix
///
/// # Returns
///
/// * QR decomposition result
///
/// # Examples
///
/// ```
/// use scirs2_core::ndarray::{array, ScalarOperand};
/// use scirs2_linalg::lapack::qr_factor;
///
/// let a = array![[2.0, 1.0], [4.0, 3.0], [8.0, 7.0]];
/// let qr_result = qr_factor(&a.view()).unwrap();
///
/// // Check that A = Q*R
/// // (implementation dependent, so not shown here)
/// ```
#[allow(dead_code)]
pub fn qr_factor<F>(a: &ArrayView2<F>) -> LinalgResult<QRDecomposition<F>>
where
    F: Float + NumAssign + Sum + Send + Sync + ScalarOperand + 'static,
{
    let n = a.nrows();
    let m = a.ncols();

    if n == 0 || m == 0 {
        return Err(LinalgError::ComputationError(
            "Empty matrix provided".to_string(),
        ));
    }

    // Make sure we have at least as many rows as columns
    if n < m {
        return Err(LinalgError::ComputationError(
            "QR decomposition requires rows >= columns".to_string(),
        ));
    }

    // Make a copy of the input matrix
    let mut r = a.to_owned();

    // Initialize Q as identity matrix
    let mut q = Array2::zeros((n, n));
    for i in 0..n {
        q[[i, i]] = F::one();
    }

    // Householder reflections
    for k in 0..m.min(n) {
        // Extract the k-th column from k-th row to bottom
        let x = r.slice(scirs2_core::ndarray::s![k.., k]).to_owned();

        // Compute the Householder vector
        let mut v = x.clone();
        let x_norm = x.iter().map(|&xi| xi * xi).sum::<F>().sqrt();

        if x_norm > F::epsilon() {
            let alpha = if x[0] >= F::zero() { -x_norm } else { x_norm };
            v[0] -= alpha;

            // Normalize v
            let v_norm = v.iter().map(|&vi| vi * vi).sum::<F>().sqrt();
            if v_norm > F::epsilon() {
                for i in 0..v.len() {
                    v[i] /= v_norm;
                }

                // Apply Householder reflection to R
                for j in k..m {
                    let column = r.slice(scirs2_core::ndarray::s![k.., j]).to_owned();
                    let dot_product = v
                        .iter()
                        .zip(column.iter())
                        .map(|(&vi, &ci)| vi * ci)
                        .fold(F::zero(), |acc, val| acc + val);

                    for i in k..n {
                        r[[i, j]] -= F::from(2.0).unwrap() * v[i - k] * dot_product;
                    }
                }

                // Apply Householder reflection to Q
                for j in 0..n {
                    let column = q.slice(scirs2_core::ndarray::s![.., j]).to_owned();
                    let dot_product = (k..n)
                        .map(|i| v[i - k] * column[i])
                        .fold(F::zero(), |acc, val| acc + val);

                    for i in k..n {
                        q[[i, j]] -= F::from(2.0).unwrap() * v[i - k] * dot_product;
                    }
                }
            }
        }
    }

    // Transpose Q to get Q instead of Q^T
    let q = q.t().to_owned();

    Ok(QRDecomposition { q, r })
}

/// Performs singular value decomposition.
///
/// # Arguments
///
/// * `a` - Input matrix
/// * `full_matrices` - Whether to return full U and V^T matrices
///
/// # Returns
///
/// * SVD decomposition result
///
/// # Examples
///
/// ```
/// use scirs2_core::ndarray::{array, ScalarOperand};
/// use scirs2_linalg::lapack::svd;
///
/// let a = array![[1.0, 2.0], [3.0, 4.0], [5.0, 6.0]];
/// let svd_result = svd(&a.view(), false).unwrap();
///
/// // Check that A = U*diag(S)*V^T
/// // (implementation dependent, so not shown here)
/// ```
#[allow(dead_code)]
pub fn svd<F>(a: &ArrayView2<F>, fullmatrices: bool) -> LinalgResult<SVDDecomposition<F>>
where
    F: Float
        + NumAssign
        + scirs2_core::ndarray::ScalarOperand
        + std::iter::Sum
        + Send
        + Sync
        + 'static,
{
    let n = a.nrows();
    let m = a.ncols();

    if n == 0 || m == 0 {
        return Err(LinalgError::ComputationError(
            "Empty matrix provided".to_string(),
        ));
    }

    // Special case for 1x1 matrix
    if n == 1 && m == 1 {
        let u = Array2::from_elem((1, 1), F::one());
        let s = array![a[[0, 0]].abs()];
        let vt = if a[[0, 0]] >= F::zero() {
            Array2::from_elem((1, 1), F::one())
        } else {
            Array2::from_elem((1, 1), -F::one())
        };
        return Ok(SVDDecomposition { u, s, vt });
    }

    // Use a more stable approach: compute SVD via eigendecomposition
    // but with better numerical stability

    // For numerical stability, choose the smaller dimension
    let use_ata = n >= m; // Use A^T*A if tall matrix, A*A^T if wide matrix

    let (eigenvalues, eigenvectors) = if use_ata {
        // Compute A^T * A for right singular vectors (when n >= m)
        let a_t = a.t();
        let ata = a_t.dot(a);

        use crate::eigen::eigh;
        match eigh(&ata.view(), None) {
            Ok(result) => result,
            Err(_) => {
                return Err(LinalgError::ComputationError(
                    "Failed to compute eigendecomposition for SVD".to_string(),
                ));
            }
        }
    } else {
        // Compute A * A^T for left singular vectors (when m > n)
        let aat = a.dot(&a.t());

        use crate::eigen::eigh;
        match eigh(&aat.view(), None) {
            Ok(result) => result,
            Err(_) => {
                return Err(LinalgError::ComputationError(
                    "Failed to compute eigendecomposition for SVD".to_string(),
                ));
            }
        }
    };

    // Sort eigenvalues in descending order and filter out negative ones
    let mut indices: Vec<usize> = (0..eigenvalues.len()).collect();
    indices.sort_by(|&i, &j| {
        eigenvalues[j]
            .partial_cmp(&eigenvalues[i])
            .unwrap_or(std::cmp::Ordering::Equal)
    });

    // Create singular values (square roots of eigenvalues, taking absolute value for stability)
    let rank = n.min(m);
    let mut s = Array1::<F>::zeros(rank);
    for (new_idx, &old_idx) in indices.iter().enumerate().take(rank) {
        s[new_idx] = eigenvalues[old_idx].abs().sqrt();
    }

    // Build U and V _matrices with improved orthogonality
    let (u, vt) = if use_ata {
        // We computed V from A^T*A, now compute U = A*V*S^(-1)
        let mut v_sorted = Array2::<F>::zeros((m, rank));
        for (new_idx, &old_idx) in indices.iter().enumerate().take(rank) {
            v_sorted
                .column_mut(new_idx)
                .assign(&eigenvectors.column(old_idx));
        }

        // Compute U with better numerical stability
        let mut u = Array2::<F>::zeros((n, rank));
        for i in 0..rank {
            if s[i] > F::from(1e-14).unwrap() {
                // More conservative threshold
                let av_col = a.dot(&v_sorted.column(i));
                let norm = av_col.dot(&av_col).sqrt();
                if norm > F::from(1e-14).unwrap() {
                    u.column_mut(i).assign(&(&av_col / norm));
                    // Recompute singular value more accurately
                    s[i] = norm;
                }
            }
        }

        // Apply modified Gram-Schmidt for better orthogonality
        modified_gram_schmidt(&mut u);

        let vt = v_sorted.t().to_owned();
        (u, vt)
    } else {
        // We computed U from A*A^T, now compute V = A^T*U*S^(-1)
        let mut u_sorted = Array2::<F>::zeros((n, rank));
        for (new_idx, &old_idx) in indices.iter().enumerate().take(rank) {
            u_sorted
                .column_mut(new_idx)
                .assign(&eigenvectors.column(old_idx));
        }

        // Compute V with better numerical stability
        let mut v = Array2::<F>::zeros((m, rank));
        for i in 0..rank {
            if s[i] > F::from(1e-14).unwrap() {
                let atv_col = a.t().dot(&u_sorted.column(i));
                let norm = atv_col.dot(&atv_col).sqrt();
                if norm > F::from(1e-14).unwrap() {
                    v.column_mut(i).assign(&(&atv_col / norm));
                    // Recompute singular value more accurately
                    s[i] = norm;
                }
            }
        }

        // Apply modified Gram-Schmidt for better orthogonality
        modified_gram_schmidt(&mut u_sorted);
        modified_gram_schmidt(&mut v);

        let vt = v.t().to_owned();
        (u_sorted, vt)
    };

    // Handle full _matrices case with better orthogonalization
    let final_u = if fullmatrices && u.ncols() < n {
        extend_to_orthogonal_basis(u, n)
    } else {
        u
    };

    let final_vt = if fullmatrices && vt.nrows() < m {
        let v_extended = extend_to_orthogonal_basis(vt.t().to_owned(), m);
        v_extended.t().to_owned()
    } else {
        vt
    };

    // Ensure singular values are sorted in descending order (required by SciPy compatibility)
    let mut sort_indices: Vec<usize> = (0..s.len()).collect();
    sort_indices.sort_by(|&i, &j| s[j].partial_cmp(&s[i]).unwrap());

    // Check if sorting is needed
    let needs_sorting = sort_indices.iter().enumerate().any(|(i, &j)| i != j);

    let (final_u_sorted, final_s, final_vt_sorted) = if needs_sorting {
        // Create sorted singular values
        let mut s_sorted = Array1::<F>::zeros(s.len());
        for (new_idx, &old_idx) in sort_indices.iter().enumerate() {
            s_sorted[new_idx] = s[old_idx];
        }

        // Reorder U columns
        let mut u_sorted = Array2::<F>::zeros(final_u.raw_dim());
        for (new_idx, &old_idx) in sort_indices.iter().enumerate() {
            if old_idx < final_u.ncols() && new_idx < u_sorted.ncols() {
                u_sorted
                    .column_mut(new_idx)
                    .assign(&final_u.column(old_idx));
            }
        }

        // Reorder Vt rows
        let mut vt_sorted = Array2::<F>::zeros(final_vt.raw_dim());
        for (new_idx, &old_idx) in sort_indices.iter().enumerate() {
            if old_idx < final_vt.nrows() && new_idx < vt_sorted.nrows() {
                vt_sorted.row_mut(new_idx).assign(&final_vt.row(old_idx));
            }
        }

        (u_sorted, s_sorted, vt_sorted)
    } else {
        (final_u, s, final_vt)
    };

    Ok(SVDDecomposition {
        u: final_u_sorted,
        s: final_s,
        vt: final_vt_sorted,
    })
}

/// Modified Gram-Schmidt orthogonalization for better numerical stability
#[allow(dead_code)]
fn modified_gram_schmidt<F>(matrix: &mut Array2<F>)
where
    F: Float
        + NumAssign
        + scirs2_core::ndarray::ScalarOperand
        + std::iter::Sum
        + Send
        + Sync
        + 'static,
{
    let n_cols = matrix.ncols();

    for i in 0..n_cols {
        // Normalize column i
        let mut col_i = matrix.column(i).to_owned();
        let norm = col_i.dot(&col_i).sqrt();

        if norm > F::from(1e-14).unwrap() {
            col_i /= norm;
            matrix.column_mut(i).assign(&col_i);

            // Orthogonalize subsequent columns against column i
            for j in (i + 1)..n_cols {
                let mut col_j = matrix.column(j).to_owned();
                let proj = col_i.dot(&col_j);
                col_j = col_j - &col_i * proj;
                matrix.column_mut(j).assign(&col_j);
            }
        }
    }
}

/// Extend a matrix to form a complete orthogonal basis
#[allow(dead_code)]
fn extend_to_orthogonal_basis<F>(matrix: Array2<F>, targetsize: usize) -> Array2<F>
where
    F: Float
        + NumAssign
        + scirs2_core::ndarray::ScalarOperand
        + std::iter::Sum
        + Send
        + Sync
        + 'static,
{
    let current_cols = matrix.ncols();
    if current_cols >= targetsize {
        return matrix;
    }

    let n_rows = matrix.nrows();
    let mut extended = Array2::<F>::zeros((n_rows, targetsize));
    extended
        .slice_mut(scirs2_core::ndarray::s![.., 0..current_cols])
        .assign(&matrix);

    // Add orthogonal vectors using QR decomposition approach
    for k in current_cols..targetsize {
        // Start with a random vector
        let mut new_vec = Array1::<F>::zeros(n_rows);
        if k < n_rows {
            new_vec[k] = F::one();
        } else {
            // Use a different approach for overcomplete case
            new_vec[k % n_rows] = F::one();
        }

        // Orthogonalize against existing columns using modified Gram-Schmidt
        for j in 0..k {
            let existing_col = extended.column(j);
            let proj = existing_col.dot(&new_vec);
            new_vec = new_vec - &existing_col * proj;
        }

        // Normalize
        let norm = new_vec.dot(&new_vec).sqrt();
        if norm > F::from(1e-14).unwrap() {
            new_vec /= norm;
        }

        extended.column_mut(k).assign(&new_vec);
    }

    // Apply final Gram-Schmidt pass for better orthogonality
    modified_gram_schmidt(&mut extended);

    extended
}

/// Computes the eigenvalues and eigenvectors of a square matrix.
///
/// # Arguments
///
/// * `a` - Input square matrix
///
/// # Returns
///
/// * Eigenvalue decomposition result
///
/// # Examples
///
/// ```
/// use scirs2_core::ndarray::{array, ScalarOperand};
/// use scirs2_linalg::lapack::eig;
///
/// let a = array![[1.0, 2.0], [3.0, 4.0]];
/// let eig_result = eig(&a.view()).unwrap();
///
/// // Check that A*V = V*diag(eigenvalues)
/// // (implementation dependent, so not shown here)
/// ```
#[allow(dead_code)]
pub fn eig<F>(a: &ArrayView2<F>) -> LinalgResult<EigDecomposition<F>>
where
    F: Float + NumAssign,
{
    // This is a placeholder implementation. A proper eigenvalue decomposition would use
    // more efficient numerical methods like the QR algorithm.

    let n = a.nrows();
    let m = a.ncols();

    if n == 0 || m == 0 {
        return Err(LinalgError::ComputationError(
            "Empty matrix provided".to_string(),
        ));
    }

    if n != m {
        return Err(LinalgError::DimensionError(
            "Matrix must be square for eigenvalue decomposition".to_string(),
        ));
    }

    // Create placeholder eigenvalues and eigenvectors
    let mut eigenvalues = Array1::zeros(n);
    let eigenvectors = Array2::eye(n);

    // For demonstration, set diagonal elements as the eigenvalues
    // This is only correct for diagonal matrices!
    for i in 0..n {
        eigenvalues[i] = a[[i, i]];
    }

    // Return the placeholder result
    // In a real implementation, we would compute the actual eigenvalues and eigenvectors
    Ok(EigDecomposition {
        eigenvalues,
        eigenvectors,
    })
}

/// Computes the Cholesky decomposition of a symmetric positive-definite matrix.
///
/// # Arguments
///
/// * `a` - Input symmetric positive-definite matrix
///
/// # Returns
///
/// * The lower triangular matrix L such that A = L*L^T
///
/// # Examples
///
/// ```
/// use scirs2_core::ndarray::{array, ScalarOperand};
/// use scirs2_linalg::lapack::cholesky;
///
/// let a = array![[4.0, 2.0], [2.0, 5.0]];
/// let l = cholesky(&a.view()).unwrap();
///
/// // Check that A = L*L^T
/// // (implementation dependent, so not shown here)
/// ```
#[allow(dead_code)]
pub fn cholesky<F>(a: &ArrayView2<F>) -> LinalgResult<Array2<F>>
where
    F: Float + NumAssign,
{
    let n = a.nrows();

    if n == 0 || a.ncols() == 0 {
        return Err(LinalgError::ComputationError(
            "Empty matrix provided".to_string(),
        ));
    }

    if n != a.ncols() {
        return Err(LinalgError::DimensionError(
            "Matrix must be square for Cholesky decomposition".to_string(),
        ));
    }

    // Initialize the result as a copy of the input
    let mut l = Array2::zeros((n, n));

    // Cholesky-Banachiewicz algorithm
    for i in 0..n {
        for j in 0..=i {
            let mut sum = F::zero();
            for k in 0..j {
                sum += l[[i, k]] * l[[j, k]];
            }

            if i == j {
                let val = a[[i, i]] - sum;
                if val <= F::zero() {
                    // Use enhanced error with regularization suggestions
                    return Err(LinalgError::non_positive_definite_with_suggestions(
                        "Cholesky decomposition",
                        a.dim(),
                        None, // Could analyze eigenvalues to count negative ones
                    ));
                }
                l[[i, j]] = val.sqrt();
            } else {
                l[[i, j]] = (a[[i, j]] - sum) / l[[j, j]];
            }
        }
    }

    Ok(l)
}

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;
    use scirs2_core::ndarray::array;

    #[test]
    fn test_lu_factor() {
        let a = array![[2.0, 1.0], [4.0, 3.0]];
        let result = lu_factor(&a.view()).unwrap();

        // Verify the specific values in our LU decomposition implementation
        // First row should be [4.0, 3.0] due to pivoting
        assert_relative_eq!(result.lu[[0, 0]], 4.0);
        assert_relative_eq!(result.lu[[0, 1]], 3.0);

        // L multiplier should be 0.5 because 2.0/4.0 = 0.5
        assert_relative_eq!(result.lu[[1, 0]], 0.5);

        // The actual value in our implementation is -0.5 due to the specific algorithm
        assert_relative_eq!(result.lu[[1, 1]], -0.5);

        // Verify that we can reconstruct the original matrix
        // Permutation should be [1, 0] meaning row 1 (the second row) was moved to position 0
        assert_eq!(result.piv[0], 1);
        assert_eq!(result.piv[1], 0);

        // Verify that we can reconstruct A from the LU decomposition
        // Create L matrix with unit diagonal
        let mut l = Array2::<f64>::zeros((2, 2));
        l[[0, 0]] = 1.0;
        l[[1, 0]] = result.lu[[1, 0]];
        l[[1, 1]] = 1.0;

        // Create U matrix
        let mut u = Array2::<f64>::zeros((2, 2));
        u[[0, 0]] = result.lu[[0, 0]];
        u[[0, 1]] = result.lu[[0, 1]];
        u[[1, 1]] = result.lu[[1, 1]];

        // Create permutation matrix
        let mut p = Array2::<f64>::zeros((2, 2));
        p[[0, result.piv[0]]] = 1.0;
        p[[1, result.piv[1]]] = 1.0;

        // Get permuted original matrix
        let pa = p.dot(&a);

        // The final matrix element may differ in sign but this should still reconstruct the original matrix
        assert_relative_eq!(pa[[0, 0]], 4.0);
        assert_relative_eq!(pa[[0, 1]], 3.0);
        assert_relative_eq!(pa[[1, 0]], 2.0);
        assert_relative_eq!(pa[[1, 1]], 1.0);
    }

    #[test]
    fn test_cholesky() {
        let a = array![[4.0, 2.0], [2.0, 5.0]];
        let l = cholesky(&a.view()).unwrap();

        // Check some elements
        assert_relative_eq!(l[[0, 0]], 2.0);
        assert_relative_eq!(l[[1, 0]], 1.0);
        assert_relative_eq!(l[[1, 1]], 2.0);

        // Verify L*L^T = A
        let lt = l.t();
        let product = l.dot(&lt);

        assert_relative_eq!(product[[0, 0]], a[[0, 0]]);
        assert_relative_eq!(product[[0, 1]], a[[0, 1]]);
        assert_relative_eq!(product[[1, 0]], a[[1, 0]]);
        assert_relative_eq!(product[[1, 1]], a[[1, 1]]);
    }

    #[test]
    fn test_qr_factor() {
        let a = array![[2.0, 1.0], [4.0, 3.0]];
        let result = qr_factor(&a.view()).unwrap();

        // Basic check of dimensions
        assert_eq!(result.q.shape(), &[2, 2]);
        assert_eq!(result.r.shape(), &[2, 2]);

        // Verify that Q is orthogonal (Q^T * Q = I)
        let qt = result.q.t();
        let q_orthogonal = qt.dot(&result.q);

        assert_relative_eq!(q_orthogonal[[0, 0]], 1.0, epsilon = 1e-5);
        assert_relative_eq!(q_orthogonal[[0, 1]], 0.0, epsilon = 1e-5);
        assert_relative_eq!(q_orthogonal[[1, 0]], 0.0, epsilon = 1e-5);
        assert_relative_eq!(q_orthogonal[[1, 1]], 1.0, epsilon = 1e-5);
    }
}