Add triangular solve

11 files changed, 344 insertions, 34 deletions

diff --git a/.gitignore b/.gitignore
index c8fac71..2ef1f3b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
 build/
 .vscode/
-.DS_Store
\ No newline at end of file
+.DS_Store
+build-release-none/
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c0a79d5..54f9d5b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,7 @@ add_library(linear_algebra
     src/vector.cpp
     src/matrix.cpp
     src/norms.cpp
+    src/triangular_solve.cpp
 )
 
 add_library(linear_algebra::core ALIAS linear_algebra)
@@ -52,12 +53,18 @@ elseif(MSVC)
 endif()
 
 if(LINEAR_ALGEBRA_BUILD_EXAMPLES)
-    add_executable(solve_linear_system examples/solve_linear_system.cpp)
-    target_link_libraries(solve_linear_system PRIVATE linear_algebra::core)
+    add_executable(linear_system examples/linear_system.cpp)
+    target_link_libraries(linear_system PRIVATE linear_algebra::core)
+
+    add_executable(matmul examples/matmul.cpp)
+    target_link_libraries(matmul PRIVATE linear_algebra::core)
 endif()
 
 if(LINEAR_ALGEBRA_BUILD_TESTS)
     include(FetchContent)
+endif()
+
+if(LINEAR_ALGEBRA_BUILD_TESTS)
 
     FetchContent_Declare(
         Catch2
@@ -72,6 +79,7 @@ if(LINEAR_ALGEBRA_BUILD_TESTS)
     add_executable(linear_algebra_tests
         tests/test_vector.cpp
         tests/test_matrix.cpp
+        tests/test_triangular_solve.cpp
     )
 
     target_link_libraries(linear_algebra_tests
diff --git a/README.md b/README.md
index 1cd42ec..e95b936 100644
--- a/README.md
+++ b/README.md
@@ -2,7 +2,7 @@
 
 This repo contains a small C++ dense numerical linear algebra library for `double`, with a companion experiments directory for evaluating performance.
 
-The current matmul uses a vectorized dot-product kernel. The implementation supports compile-time SIMD backends for AVX, AVX2, AVX512, NEON (AArch64/ARM64).
+The current matmul uses a vectorized dot-product kernel. The implementation supports compile-time SIMD backends for `AVX`, `AVX2`, `AVX512`, and `NEON` on `AArch64`/`ARM64` with FP64 vector support.
 
 ## Build
 
@@ -25,8 +25,9 @@ Valid values are `AUTO`, `NONE`, `AVX`, `AVX2`, and `AVX512`. `AUTO` uses the co
 ctest --test-dir build --output-on-failure
 ```
 
-## Run the example
+## Run examples
 
 ```bash
-./build/solve_linear_system
+./build/linear_system
+./build/matmul
 ```
\ No newline at end of file
diff --git a/examples/linear_system.cpp b/examples/linear_system.cpp
new file mode 100644
index 0000000..184f576
--- /dev/null
+++ b/examples/linear_system.cpp
@@ -0,0 +1,36 @@
+#include "matrix.hpp"
+#include "norms.hpp"
+#include "triangular_solve.hpp"
+#include "vector.hpp"
+
+#include <iomanip>
+#include <iostream>
+
+int main() {
+    const linalg::Matrix basis = linalg::Matrix::identity(3);
+    const linalg::Matrix upper{
+        {4.0, -2.0, 1.0},
+        {0.0, 3.0, 5.0},
+        {0.0, 0.0, -2.0}
+    };
+    const linalg::Vector expected{2.0, -1.0, 3.0};
+    const linalg::Vector rhs = upper * expected;
+    const linalg::Vector x = linalg::backward_substitution(upper, rhs);
+
+    std::cout << "Week 3 triangular solve demo\n";
+    std::cout << "Identity matrix diagonal: ";
+    for (std::size_t i = 0; i < basis.rows(); ++i) {
+        std::cout << basis(i, i) << (i + 1 == basis.rows() ? '\n' : ' ');
+    }
+
+    std::cout << "Recovered solution x: ";
+    for (std::size_t i = 0; i < x.size(); ++i) {
+        std::cout << std::fixed << std::setprecision(2) << x[i]
+                  << (i + 1 == x.size() ? '\n' : ' ');
+    }
+
+    const linalg::Vector residual = (upper * x) - rhs;
+    std::cout << "Residual 2-norm = " << linalg::norm2(residual) << '\n';
+
+    return 0;
+}
diff --git a/examples/matmul.cpp b/examples/matmul.cpp
new file mode 100644
index 0000000..c6245f3
--- /dev/null
+++ b/examples/matmul.cpp
@@ -0,0 +1,32 @@
+#include "matrix.hpp"
+
+#include <iomanip>
+#include <iostream>
+
+int main() {
+    const linalg::Matrix a{
+        {1.0, 2.0, 3.0},
+        {4.0, 5.0, 6.0}
+    };
+    const linalg::Matrix b{
+        {7.0, 8.0},
+        {9.0, 10.0},
+        {11.0, 12.0}
+    };
+
+    const linalg::Matrix c = a * b;
+
+    std::cout << "Matrix multiplication\n";
+    std::cout << "A is " << a.rows() << " x " << a.cols() << '\n';
+    std::cout << "B is " << b.rows() << " x " << b.cols() << '\n';
+    std::cout << "C = A * B:\n";
+
+    for (std::size_t i = 0; i < c.rows(); ++i) {
+        for (std::size_t j = 0; j < c.cols(); ++j) {
+            std::cout << std::fixed << std::setprecision(2) << c(i, j)
+                      << (j + 1 == c.cols() ? '\n' : ' ');
+        }
+    }
+
+    return 0;
+}
diff --git a/examples/solve_linear_system.cpp b/examples/solve_linear_system.cpp
deleted file mode 100644
index af45094..0000000
--- a/examples/solve_linear_system.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-#include "matrix.hpp"
-#include "vector.hpp"
-
-#include <iomanip>
-#include <iostream>
-
-int main() {
-    const linalg::Matrix basis = linalg::Matrix::identity(3);
-    const linalg::Vector x{1.0, -2.0, 0.5};
-
-    std::cout << "Identity matrix diagonal: ";
-    for (std::size_t i = 0; i < basis.rows(); ++i) {
-        std::cout << basis(i, i) << (i + 1 == basis.rows() ? '\n' : ' ');
-    }
-
-    std::cout << "Vector x: ";
-    for (std::size_t i = 0; i < x.size(); ++i) {
-        std::cout << std::fixed << std::setprecision(2) << x[i]
-                  << (i + 1 == x.size() ? '\n' : ' ');
-    }
-
-    std::cout << "x dot x = " << linalg::dot(x, x) << '\n';
-
-    return 0;
-}
diff --git a/include/linalg_error.hpp b/include/linalg_error.hpp
index 8258899..25c48ad 100644
--- a/include/linalg_error.hpp
+++ b/include/linalg_error.hpp
@@ -16,4 +16,10 @@ public:
         : LinAlgError(message) {}
 };
 
+class SingularMatrixError : public LinAlgError {
+public:
+    explicit SingularMatrixError(const std::string& message)
+        : LinAlgError(message) {}
+};
+
 }  // namespace linalg
diff --git a/include/triangular_solve.hpp b/include/triangular_solve.hpp
new file mode 100644
index 0000000..632f225
--- /dev/null
+++ b/include/triangular_solve.hpp
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <cstddef>
+
+#include "matrix.hpp"
+#include "vector.hpp"
+
+namespace linalg {
+
+Vector forward_substitution(
+    const Matrix& lower,
+    const Vector& rhs,
+    double singular_tolerance = 1e-12,
+    bool unit_diagonal = false);
+
+Vector backward_substitution(
+    const Matrix& upper,
+    const Vector& rhs,
+    double singular_tolerance = 1e-12,
+    bool unit_diagonal = false);
+
+}  // namespace linalg
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 <immintrin.h>
 #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 <arm_neon.h>
 #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 <cmath>
+#include <sstream>
+#include <stdexcept>
+
+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
diff --git a/tests/test_triangular_solve.cpp b/tests/test_triangular_solve.cpp
new file mode 100644
index 0000000..da7e55c
--- /dev/null
+++ b/tests/test_triangular_solve.cpp
@@ -0,0 +1,101 @@
+#include "linalg_error.hpp"
+#include "matrix.hpp"
+#include "norms.hpp"
+#include "triangular_solve.hpp"
+
+#include <catch2/catch_approx.hpp>
+#include <catch2/catch_test_macros.hpp>
+
+using linalg::DimensionMismatchError;
+using linalg::Matrix;
+using linalg::SingularMatrixError;
+using linalg::Vector;
+
+namespace {
+
+double residual_norm(const Matrix& a, const Vector& x, const Vector& b) {
+    return linalg::norm2((a * x) - b);
+}
+
+}  // namespace
+
+TEST_CASE("Forward substitution solves lower-triangular systems", "[triangular]") {
+    const Matrix lower{
+        {2.0, 0.0, 0.0},
+        {-1.0, 3.0, 0.0},
+        {4.0, 2.0, 1.0}
+    };
+    const Vector expected{1.0, 2.0, -1.0};
+    const Vector rhs = lower * expected;
+
+    const Vector x = linalg::forward_substitution(lower, rhs);
+    REQUIRE(x.size() == expected.size());
+    CHECK(x[0] == Catch::Approx(expected[0]));
+    CHECK(x[1] == Catch::Approx(expected[1]));
+    CHECK(x[2] == Catch::Approx(expected[2]));
+    CHECK(residual_norm(lower, x, rhs) == Catch::Approx(0.0).margin(1e-12));
+}
+
+TEST_CASE("Backward substitution solves upper-triangular systems", "[triangular]") {
+    const Matrix upper{
+        {4.0, -2.0, 1.0},
+        {0.0, 3.0, 5.0},
+        {0.0, 0.0, -2.0}
+    };
+    const Vector expected{2.0, -1.0, 3.0};
+    const Vector rhs = upper * expected;
+
+    const Vector x = linalg::backward_substitution(upper, rhs);
+    REQUIRE(x.size() == expected.size());
+    CHECK(x[0] == Catch::Approx(expected[0]));
+    CHECK(x[1] == Catch::Approx(expected[1]));
+    CHECK(x[2] == Catch::Approx(expected[2]));
+    CHECK(residual_norm(upper, x, rhs) == Catch::Approx(0.0).margin(1e-12));
+}
+
+TEST_CASE("Triangular solves support unit-diagonal systems", "[triangular]") {
+    const Matrix lower{
+        {1.0, 0.0, 0.0},
+        {-2.0, 1.0, 0.0},
+        {3.0, -1.0, 1.0}
+    };
+    const Vector rhs{1.0, 0.0, 4.0};
+
+    const Vector x = linalg::forward_substitution(lower, rhs, 1e-12, true);
+    CHECK(x[0] == Catch::Approx(1.0));
+    CHECK(x[1] == Catch::Approx(2.0));
+    CHECK(x[2] == Catch::Approx(3.0));
+}
+
+TEST_CASE("Triangular solves reject shape and structure mismatches", "[triangular]") {
+    const Matrix nonsquare(2, 3);
+    const Vector rhs2{1.0, 2.0};
+    CHECK_THROWS_AS(linalg::forward_substitution(nonsquare, rhs2), DimensionMismatchError);
+
+    const Matrix lower{
+        {1.0, 1.0},
+        {2.0, 3.0}
+    };
+    CHECK_THROWS_AS(linalg::forward_substitution(lower, rhs2), std::invalid_argument);
+
+    const Matrix upper{
+        {1.0, 2.0},
+        {1.0, 3.0}
+    };
+    CHECK_THROWS_AS(linalg::backward_substitution(upper, rhs2), std::invalid_argument);
+}
+
+TEST_CASE("Triangular solves detect negligible diagonal entries", "[triangular]") {
+    const Matrix lower{
+        {1e-14, 0.0},
+        {2.0, 1.0}
+    };
+    const Vector rhs{1.0, 2.0};
+    CHECK_THROWS_AS(linalg::forward_substitution(lower, rhs), SingularMatrixError);
+
+    const Matrix upper{
+        {1.0, 2.0},
+        {0.0, 1e-14}
+    };
+    CHECK_THROWS_AS(linalg::backward_substitution(upper, rhs), SingularMatrixError);
+}