From 7db0d82731ababdf0b2f4d986dd54da3b4650954 Mon Sep 17 00:00:00 2001 From: y-jan137 Date: Sat, 14 Mar 2026 14:49:46 +0300 Subject: Add triangular solve --- src/matrix.cpp | 6 +-- src/triangular_solve.cpp | 128 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 131 insertions(+), 3 deletions(-) create mode 100644 src/triangular_solve.cpp (limited to 'src') diff --git a/src/matrix.cpp b/src/matrix.cpp index 1faab25..c27264b 100644 --- a/src/matrix.cpp +++ b/src/matrix.cpp @@ -11,7 +11,7 @@ #include #endif -#if !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && !defined(__clangd__) +#if !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && defined(__ARM_FEATURE_FP64_VECTOR_ARITHMETIC) #include #endif @@ -114,7 +114,7 @@ double dot_product_avx(const double* lhs, const double* rhs, std::size_t count) } #endif -#if !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && !defined(__clangd__) +#if !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && defined(__ARM_FEATURE_FP64_VECTOR_ARITHMETIC) double horizontal_sum(float64x2_t values) { return vgetq_lane_f64(values, 0) + vgetq_lane_f64(values, 1); } @@ -146,7 +146,7 @@ double dot_product_simd(const double* lhs, const double* rhs, std::size_t count) return dot_product_avx2(lhs, rhs, count); #elif !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__AVX__) && !defined(__AVX2__) return dot_product_avx(lhs, rhs, count); -#elif !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && !defined(__clangd__) +#elif !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && defined(__ARM_FEATURE_FP64_VECTOR_ARITHMETIC) return dot_product_neon(lhs, rhs, count); #else return dot_product_scalar(lhs, rhs, count); diff --git a/src/triangular_solve.cpp b/src/triangular_solve.cpp new file mode 100644 index 0000000..6a5e8ad --- /dev/null +++ b/src/triangular_solve.cpp @@ -0,0 +1,128 @@ +#include "triangular_solve.hpp" + +#include "linalg_error.hpp" + +#include +#include +#include + +namespace linalg { + +namespace { + +void validate_square_system(const Matrix& matrix, const Vector& rhs, const char* operation) { + if (matrix.rows() != matrix.cols()) { + std::ostringstream oss; + oss << operation << " requires a square matrix, got " << matrix.rows() << "x" + << matrix.cols(); + throw DimensionMismatchError(oss.str()); + } + + if (matrix.rows() != rhs.size()) { + std::ostringstream oss; + oss << operation << " requires matrix dimension to match rhs size, got " + << matrix.rows() << " and " << rhs.size(); + throw DimensionMismatchError(oss.str()); + } +} + +void validate_tolerance(double singular_tolerance) { + if (singular_tolerance < 0.0) { + throw std::invalid_argument("Singular tolerance must be nonnegative"); + } +} + +void validate_lower_triangular( + const Matrix& lower, + double singular_tolerance, + bool unit_diagonal) { + for (std::size_t i = 0; i < lower.rows(); ++i) { + for (std::size_t j = i + 1; j < lower.cols(); ++j) { + if (std::abs(lower(i, j)) > singular_tolerance) { + throw std::invalid_argument( + "Forward substitution requires a lower-triangular matrix"); + } + } + + if (!unit_diagonal && std::abs(lower(i, i)) <= singular_tolerance) { + throw SingularMatrixError( + "Forward substitution encountered a zero or tiny diagonal entry"); + } + } +} + +void validate_upper_triangular( + const Matrix& upper, + double singular_tolerance, + bool unit_diagonal) { + for (std::size_t i = 0; i < upper.rows(); ++i) { + for (std::size_t j = 0; j < i; ++j) { + if (std::abs(upper(i, j)) > singular_tolerance) { + throw std::invalid_argument( + "Backward substitution requires an upper-triangular matrix"); + } + } + + if (!unit_diagonal && std::abs(upper(i, i)) <= singular_tolerance) { + throw SingularMatrixError( + "Backward substitution encountered a negligible diagonal entry"); + } + } +} + +} // namespace + +Vector forward_substitution( + const Matrix& lower, + const Vector& rhs, + double singular_tolerance, + bool unit_diagonal) { + validate_tolerance(singular_tolerance); + validate_square_system(lower, rhs, "Forward substitution"); + validate_lower_triangular(lower, singular_tolerance, unit_diagonal); + + Vector solution(lower.rows()); + for (std::size_t i = 0; i < lower.rows(); ++i) { + double sum = rhs[i]; + for (std::size_t j = 0; j < i; ++j) { + sum -= lower(i, j) * solution[j]; + } + + if (unit_diagonal) { + solution[i] = sum; + } else { + solution[i] = sum / lower(i, i); + } + } + + return solution; +} + +Vector backward_substitution( + const Matrix& upper, + const Vector& rhs, + double singular_tolerance, + bool unit_diagonal) { + validate_tolerance(singular_tolerance); + validate_square_system(upper, rhs, "Backward substitution"); + validate_upper_triangular(upper, singular_tolerance, unit_diagonal); + + Vector solution(upper.rows()); + for (std::size_t offset = 0; offset < upper.rows(); ++offset) { + const std::size_t i = upper.rows() - 1 - offset; + double sum = rhs[i]; + for (std::size_t j = i + 1; j < upper.cols(); ++j) { + sum -= upper(i, j) * solution[j]; + } + + if (unit_diagonal) { + solution[i] = sum; + } else { + solution[i] = sum / upper(i, i); + } + } + + return solution; +} + +} // namespace linalg -- cgit v1.2.3