diff --git a/examples/full_dimensional_polytope/CMakeLists.txt b/examples/full_dimensional_polytope/CMakeLists.txt new file mode 100644 index 000000000..0baba7739 --- /dev/null +++ b/examples/full_dimensional_polytope/CMakeLists.txt @@ -0,0 +1,111 @@ +# VolEsti (volume computation and sampling library) + +# Copyright (c) 2012-2025 Vissarion Fisikopoulos +# Copyright (c) 2018-2025 Apostolos Chalkis + +# Licensed under GNU LGPL.3, see LICENCE file + +project( VolEsti ) + +CMAKE_MINIMUM_REQUIRED(VERSION 3.11) + +set(CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS true) + +# Locate Intel MKL root (in case it is enabled) +if (APPLE) + set(MKLROOT /opt/intel/oneapi/mkl/latest) +elseif(UNIX) + set(MKLROOT /opt/intel/oneapi/mkl/latest) +endif() + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 NEW) +endif(COMMAND cmake_policy) + +option(DISABLE_NLP_ORACLES "Disable non-linear oracles (used in collocation)" ON) +option(BUILTIN_EIGEN "Use eigen from ../../external" OFF) +option(USE_MKL "Use MKL library to build eigen" OFF) + +if(DISABLE_NLP_ORACLES) + add_definitions(-DDISABLE_NLP_ORACLES) +else() + find_library(IFOPT NAMES libifopt_core.so PATHS /usr/local/lib) + find_library(IFOPT_IPOPT NAMES libifopt_ipopt.so PATHS /usr/local/lib) + find_library(GMP NAMES libgmp.so PATHS /usr/lib/x86_64-linux-gnu /usr/lib/i386-linux-gnu) + find_library(MPSOLVE NAMES libmps.so PATHS /usr/local/lib) + find_library(PTHREAD NAMES libpthread.so PATHS /usr/lib/x86_64-linux-gnu /usr/lib/i386-linux-gnu) + find_library(FFTW3 NAMES libfftw3.so.3 PATHS /usr/lib/x86_64-linux-gnu /usr/lib/i386-linux-gnu) + + if (NOT IFOPT) + message(FATAL_ERROR "This program requires the ifopt library, and will not be compiled.") + elseif (NOT GMP) + message(FATAL_ERROR "This program requires the gmp library, and will not be compiled.") + elseif (NOT MPSOLVE) + message(FATAL_ERROR "This program requires the mpsolve library, and will not be compiled.") + elseif (NOT FFTW3) + message(FATAL_ERROR "This program requires the fftw3 library, and will not be compiled.") + else() + message(STATUS "Library ifopt found: ${IFOPT}") + message(STATUS "Library gmp found: ${GMP}") + message(STATUS "Library mpsolve found: ${MPSOLVE}") + message(STATUS "Library fftw3 found:" ${FFTW3}) + endif(NOT IFOPT) +endif(DISABLE_NLP_ORACLES) + +# Include CMake modules for dependencies +include("../../external/cmake-files/Eigen.cmake") +GetEigen() + +include("../../external/cmake-files/Boost.cmake") +GetBoost() + +include("../../external/cmake-files/LPSolve.cmake") +GetLPSolve() + +include("../../external/cmake-files/SuiteSparse.cmake") +GetSuiteSparse() + +set(CMAKE_EXPORT_COMPILE_COMMANDS "ON") + +if (USE_MKL) + find_library(BLAS NAMES libblas.so libblas.dylib PATHS /usr/local/Cellar/lapack/3.9.1_1/lib /usr/lib/x86_64-linux-gnu /usr/lib/i386-linux-gnu /usr/local/Cellar/openblas/0.3.15_1/lib /usr/lib) + find_library(GFORTRAN NAME libgfortran.dylib PATHS /usr/local/Cellar/gcc/10.2.0_4/lib/gcc/10) + find_library(LAPACK NAME liblapack.dylib PATHS /usr/lib) + find_library(OPENMP NAME libiomp5.dylib PATHS /opt/intel/oneapi/compiler/2021.1.1/mac/compiler/lib) + + include_directories (BEFORE ${MKLROOT}/include) + set(PROJECT_LIBS ${BLAS_LIBRARIES} ${LAPACK_LIBRARIES} ${GFORTRAN_LIBRARIES}) + set(MKL_LINK "-L${MKLROOT}/lib -Wl,-rpath,${MKLROOT}/lib -lmkl_intel_ilp64 -lmkl_sequential -lmkl_core -lpthread -lm -ldl") + add_definitions(-DEIGEN_USE_MKL_ALL) +else() + set(MKL_LINK "") +endif(USE_MKL) + +# Include directories +include_directories (BEFORE ../../external) +include_directories (BEFORE ../../external/minimum_ellipsoid) +include_directories (BEFORE ../../include/) + +# for Eigen +if (${CMAKE_VERSION} VERSION_LESS "3.12.0") + add_compile_options(-D "EIGEN_NO_DEBUG") +else () + add_compile_definitions("EIGEN_NO_DEBUG") +endif () + +# Compiler flags +add_definitions(${CMAKE_CXX_FLAGS} "-std=c++17") # enable C++17 standard +add_definitions(${CMAKE_CXX_FLAGS} "-O3") # optimization +add_definitions(${CXX_COVERAGE_COMPILE_FLAGS} "-lm") +add_definitions(${CXX_COVERAGE_COMPILE_FLAGS} "-ldl") +add_definitions(${CXX_COVERAGE_COMPILE_FLAGS} "-DBOOST_NO_AUTO_PTR") + +# Build the example executable +add_executable(full_dimensional_example full_dimensional_example.cpp) + +# Link libraries (including SuiteSparse for the full_dimensional_polytope function) +target_link_libraries(full_dimensional_example + lp_solve + ${SUITESPARSE_LIBRARIES} + ${MKL_LINK} +) diff --git a/examples/full_dimensional_polytope/README.md b/examples/full_dimensional_polytope/README.md new file mode 100644 index 000000000..68fc83b54 --- /dev/null +++ b/examples/full_dimensional_polytope/README.md @@ -0,0 +1,79 @@ +# Full Dimensional Polytope Examples + +This directory contains examples demonstrating the `compute_full_dimensional_polytope` function from `include/preprocess/full_dimensional_polytope.hpp`. + +## Overview + +The `compute_full_dimensional_polytope` function transforms a potentially lower-dimensional polytope defined by: +- **Equality constraints**: `Aeq * x = beq` +- **Inequality constraints**: `A * x <= b` + +Into a full-dimensional polytope: `A_full * y <= b_full` + +The transformation provides: +- **shift**: A point satisfying the equality constraints +- **N**: Nullspace basis matrix with orthonormal columns +- **Mapping**: `x = shift + N * y` (from reduced space to original space) + +## Building the Example + +From this directory: + +```bash +mkdir build +cd build +cmake .. +make +``` + +## Running the Example + +```bash +./full_dimensional_example +``` + +The program will run through 6 examples, pausing between each for you to review the output. + +## Output Format + +For each example, the program displays: + +1. **Description**: What the example demonstrates +2. **Input**: The equality and inequality constraints +3. **Results**: + - Original and reduced dimensions + - Shift vector + - Nullspace basis N + - Output polytope dimensions + - Orthonormality verification + +## Key Features Demonstrated + +- ✅ Automatic dimension reduction +- ✅ Orthonormal nullspace basis +- ✅ Handling of sparse constraints +- ✅ Multiple equality constraints +- ✅ Extreme reductions (nD → 1D) +- ✅ Real-world polytopes (Birkhoff) + +## Mathematical Background + +Given a polytope `P = {x : Aeq*x = beq, A*x <= b}` which is lower-dimensional (rank(Aeq) > 0), the function computes: + +1. **Shift vector**: Solves `Aeq * shift = beq` using SPQR +2. **Nullspace basis**: Computes `N` such that `Aeq * N = 0` via QR factorization +3. **Transformed polytope**: `P' = {y : A*N*y <= b - A*shift}` + +Any point `y` in `P'` maps to a feasible point in `P` via: `x = shift + N*y` + +## Dependencies + +- Eigen3 (matrix operations) +- SuiteSparse (SPQR for QR factorization, CHOLMOD for linear systems) +- Boost (for polytope generators) +- lp_solve (for polytope operations) + +## See Also + +- **Header file**: `include/preprocess/full_dimensional_polytope.hpp` +- **Tests**: `test/full_dimensional_polytope_test.cpp` diff --git a/examples/full_dimensional_polytope/full_dimensional_example.cpp b/examples/full_dimensional_polytope/full_dimensional_example.cpp new file mode 100644 index 000000000..2440bb861 --- /dev/null +++ b/examples/full_dimensional_polytope/full_dimensional_example.cpp @@ -0,0 +1,386 @@ +// VolEsti (volume computation and sampling library) + +// Copyright (c) 2012-2025 Vissarion Fisikopoulos +// Copyright (c) 2018-2025 Apostolos Chalkis + +// Licensed under GNU LGPL.3, see LICENCE file + +#include +#include +#include +#include + +#include "preprocess/full_dimensional_polytope.hpp" +#include "generators/known_polytope_generators.h" +#include "convex_bodies/hpolytope.h" +#include "cartesian_geom/cartesian_kernel.h" + +typedef double NT; +typedef Eigen::Matrix MT; +typedef Eigen::Matrix VT; +typedef Eigen::SparseMatrix SpMT; +typedef Cartesian Kernel; +typedef typename Kernel::Point Point; +typedef HPolytope Hpolytope; + +// Helper function to create sparse matrix from dense +SpMT dense_to_sparse(const MT& dense) +{ + SpMT sparse = dense.sparseView(); + sparse.makeCompressed(); + return sparse; +} + +// Helper function to print section header +void print_header(const std::string& title) +{ + std::cout << "\n" << std::string(70, '=') << std::endl; + std::cout << " " << title << std::endl; + std::cout << std::string(70, '=') << std::endl; +} + +// Helper function to print results +void print_results(int original_dim, int reduced_dim, const VT& shift, const MT& N, + const MT& A_full, const VT& b_full) +{ + std::cout << "\nRESULTS:" << std::endl; + std::cout << "--------" << std::endl; + std::cout << "Original dimension: " << original_dim << std::endl; + std::cout << "Reduced dimension: " << reduced_dim << std::endl; + std::cout << "Dimension reduction: " << original_dim << "D -> " << reduced_dim << "D" << std::endl; + + std::cout << "\nShift vector (point satisfying Aeq*x = beq):" << std::endl; + std::cout << shift.transpose() << std::endl; + + std::cout << "\nNullspace basis N (" << N.rows() << " x " << N.cols() << "):" << std::endl; + std::cout << N << std::endl; + + std::cout << "\nOutput polytope: A_full * y <= b_full" << std::endl; + std::cout << " A_full dimensions: " << A_full.rows() << " x " << A_full.cols() << std::endl; + std::cout << " Number of constraints: " << A_full.rows() << std::endl; + + // Verify orthonormality + MT NtN = N.transpose() * N; + MT I = MT::Identity(N.cols(), N.cols()); + NT orthogonality_error = (NtN - I).cwiseAbs().maxCoeff(); + std::cout << "\nNullspace orthonormality check:" << std::endl; + std::cout << " ||N^T * N - I||_inf = " << orthogonality_error << std::endl; + std::cout << " Columns are orthonormal: " << (orthogonality_error < 1e-6 ? "YES" : "NO") << std::endl; +} + +/** + * Example 1: Canonical Simplex + * The standard (n-1)-simplex embedded in n-dimensional space + * Equality: x1 + x2 + ... + xn = 1 + * Inequalities: xi >= 0 for all i + */ +void example_canonical_simplex() +{ + print_header("Example 1: Canonical Simplex (2D triangle in 3D space)"); + + std::cout << "\nDESCRIPTION:" << std::endl; + std::cout << "The canonical 2-simplex (triangle) in 3D satisfies:" << std::endl; + std::cout << " Equality constraint: x + y + z = 1" << std::endl; + std::cout << " Inequality constraints: x, y, z >= 0" << std::endl; + std::cout << "This is a 2D surface (triangle) embedded in 3D space." << std::endl; + + int n = 3; + + // Equality constraint: x + y + z = 1 + MT Aeq_dense(1, n); + Aeq_dense << 1, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 1; + + // Inequality constraints: x, y, z >= 0 + MT A(n, n); + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1; + + VT b = VT::Zero(n); + + std::cout << "\nINPUT:" << std::endl; + std::cout << " Aeq (equality constraints):" << std::endl; + std::cout << " " << Aeq_dense << std::endl; + std::cout << " beq:" << std::endl; + std::cout << " " << beq.transpose() << std::endl; + + // Compute full dimensional polytope + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + print_results(n, N.cols(), shift, N, A_full, b_full); +} + +/** + * Example 2: Hyperplane intersecting a cube + * A plane cutting through a unit cube + */ +void example_plane_cube_intersection() +{ + print_header("Example 2: Plane intersecting unit cube"); + + std::cout << "\nDESCRIPTION:" << std::endl; + std::cout << "A plane x + y + z = 1.5 intersecting the unit cube [0,1]^3." << std::endl; + std::cout << "The intersection is a 2D hexagon embedded in 3D." << std::endl; + + int n = 3; + + // Equality constraint: x + y + z = 1.5 + MT Aeq_dense(1, n); + Aeq_dense << 1, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 1.5; + + // Inequality constraints: 0 <= x, y, z <= 1 + MT A(6, n); + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1; + + VT b(6); + b << 0, 0, 0, 1, 1, 1; + + std::cout << "\nINPUT:" << std::endl; + std::cout << " Equality: x + y + z = 1.5" << std::endl; + std::cout << " Inequalities: 0 <= x, y, z <= 1 (unit cube)" << std::endl; + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + print_results(n, N.cols(), shift, N, A_full, b_full); +} + +/** + * Example 3: Multiple equality constraints + * Reducing 4D to 2D with two independent constraints + */ +void example_multiple_constraints() +{ + print_header("Example 3: Multiple equality constraints (4D -> 2D)"); + + std::cout << "\nDESCRIPTION:" << std::endl; + std::cout << "Two independent equality constraints in 4D:" << std::endl; + std::cout << " x1 + x2 = 2" << std::endl; + std::cout << " x3 + x4 = 3" << std::endl; + std::cout << "With bounds: 0 <= xi <= 2 for all i" << std::endl; + std::cout << "This reduces the problem from 4D to 2D." << std::endl; + + int n = 4; + int m = 2; + + // Two equality constraints + MT Aeq_dense(m, n); + Aeq_dense << 1, 1, 0, 0, + 0, 0, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(m); + beq << 2, 3; + + // Inequality constraints: 0 <= xi <= 2 + MT A(2*n, n); + A.topRows(n) = -MT::Identity(n, n); + A.bottomRows(n) = MT::Identity(n, n); + + VT b(2*n); + b.head(n).setZero(); + b.tail(n).setConstant(2); + + std::cout << "\nINPUT:" << std::endl; + std::cout << " Aeq (two equality constraints):" << std::endl; + std::cout << Aeq_dense << std::endl; + std::cout << " beq: " << beq.transpose() << std::endl; + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + print_results(n, N.cols(), shift, N, A_full, b_full); +} + +/** + * Example 4: Sparse equality constraint + * Demonstrating efficient handling of sparse constraints + */ +void example_sparse_constraint() +{ + print_header("Example 4: Sparse equality constraint (5D -> 4D)"); + + std::cout << "\nDESCRIPTION:" << std::endl; + std::cout << "A sparse constraint involving only first and last variables:" << std::endl; + std::cout << " x1 + x5 = 2" << std::endl; + std::cout << "With bounds: 0 <= xi <= 2 for all i" << std::endl; + std::cout << "This is common in network flow or coupling constraints." << std::endl; + + int n = 5; + + // Sparse equality: only x1 and x5 + MT Aeq_dense = MT::Zero(1, n); + Aeq_dense(0, 0) = 1; + Aeq_dense(0, 4) = 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 2; + + // Box constraints + MT A(2*n, n); + A.topRows(n) = -MT::Identity(n, n); + A.bottomRows(n) = MT::Identity(n, n); + + VT b(2*n); + b.head(n).setZero(); + b.tail(n).setConstant(2); + + std::cout << "\nINPUT:" << std::endl; + std::cout << " Sparse equality: x1 + x5 = 2" << std::endl; + std::cout << " (variables x2, x3, x4 are unconstrained by equality)" << std::endl; + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + print_results(n, N.cols(), shift, N, A_full, b_full); +} + +/** + * Example 5: Line segment in 2D + * Extreme case: reducing to 1D + */ +void example_line_segment() +{ + print_header("Example 5: Line segment (1D line in 2D plane)"); + + std::cout << "\nDESCRIPTION:" << std::endl; + std::cout << "The line x + y = 1 within the square [0,1]^2." << std::endl; + std::cout << "This is the most extreme reduction: 2D -> 1D." << std::endl; + + int n = 2; + + // Equality: x + y = 1 + MT Aeq_dense(1, n); + Aeq_dense << 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 1; + + // Box constraints + MT A(4, n); + A << -1, 0, + 1, 0, + 0, -1, + 0, 1; + + VT b(4); + b << 0, 1, 0, 1; + + std::cout << "\nINPUT:" << std::endl; + std::cout << " Equality: x + y = 1" << std::endl; + std::cout << " Inequalities: 0 <= x, y <= 1" << std::endl; + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + print_results(n, N.cols(), shift, N, A_full, b_full); +} + +/** + * Example 6: Birkhoff polytope + * Real-world example: doubly stochastic matrices + */ +void example_birkhoff_polytope() +{ + print_header("Example 6: Birkhoff Polytope (doubly stochastic matrices)"); + + std::cout << "\nDESCRIPTION:" << std::endl; + std::cout << "The Birkhoff polytope B_3 consists of 3x3 doubly stochastic matrices:" << std::endl; + std::cout << " - Each row sums to 1" << std::endl; + std::cout << " - Each column sums to 1" << std::endl; + std::cout << " - All entries are non-negative" << std::endl; + std::cout << "This is a 9D space with 6 equality constraints -> 3D polytope" << std::endl; + + // Generate Birkhoff polytope of size 3 + Hpolytope birkhoff = generate_birkhoff(3); + + int n = birkhoff.dimension(); // Should be 9 for 3x3 matrices + MT A = birkhoff.get_mat(); + VT b = birkhoff.get_vec(); + + std::cout << "\nOriginal polytope dimension: " << n << std::endl; + std::cout << "Number of inequality constraints: " << A.rows() << std::endl; + + // For demonstration, let's use a simple equality: sum of all entries = 3 + MT Aeq_dense = MT::Ones(1, n); + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 3; // Sum of all entries in a 3x3 doubly stochastic matrix + + std::cout << "\nUsing simple equality constraint: sum of all entries = 3" << std::endl; + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + print_results(n, N.cols(), shift, N, A_full, b_full); +} + +int main() +{ + std::cout << std::string(70, '=') << std::endl; + std::cout << " FULL DIMENSIONAL POLYTOPE EXAMPLES" << std::endl; + std::cout << " Transforming lower-dimensional polytopes to full dimension" << std::endl; + std::cout << std::string(70, '=') << std::endl; + + std::cout << "\nOVERVIEW:" << std::endl; + std::cout << "These examples demonstrate the compute_full_dimensional_polytope function," << std::endl; + std::cout << "which transforms a polytope defined by:" << std::endl; + std::cout << " - Equality constraints: Aeq * x = beq" << std::endl; + std::cout << " - Inequality constraints: A * x <= b" << std::endl; + std::cout << "into a full-dimensional polytope: A_full * y <= b_full" << std::endl; + std::cout << "\nThe transformation provides:" << std::endl; + std::cout << " - shift: A point satisfying the equality constraints" << std::endl; + std::cout << " - N: Nullspace basis (orthonormal columns)" << std::endl; + std::cout << " - Mapping: x = shift + N * y" << std::endl; + + std::cout << "\n" << std::string(70, '-') << std::endl; + std::cout << "Press Enter to run each example..." << std::endl; + std::cin.get(); + + // Run all examples + example_canonical_simplex(); + std::cout << "\nPress Enter for next example..."; + std::cin.get(); + + example_plane_cube_intersection(); + std::cout << "\nPress Enter for next example..."; + std::cin.get(); + + example_multiple_constraints(); + std::cout << "\nPress Enter for next example..."; + std::cin.get(); + + example_sparse_constraint(); + std::cout << "\nPress Enter for next example..."; + std::cin.get(); + + example_line_segment(); + std::cout << "\nPress Enter for next example..."; + std::cin.get(); + + example_birkhoff_polytope(); + + std::cout << "\n" << std::string(70, '=') << std::endl; + std::cout << " ALL EXAMPLES COMPLETED" << std::endl; + std::cout << std::string(70, '=') << std::endl; + + std::cout << "\nKEY TAKEAWAYS:" << std::endl; + std::cout << "1. The function handles various types of equality constraints" << std::endl; + std::cout << "2. Dimension reduction is automatic and efficient" << std::endl; + std::cout << "3. The nullspace basis N has orthonormal columns" << std::endl; + std::cout << "4. Sparse constraints are preserved where possible" << std::endl; + std::cout << "5. The transformation preserves feasibility and structure" << std::endl; + + return 0; +} diff --git a/external/cmake-files/README_SuiteSparse.md b/external/cmake-files/README_SuiteSparse.md new file mode 100644 index 000000000..12af5962c --- /dev/null +++ b/external/cmake-files/README_SuiteSparse.md @@ -0,0 +1,65 @@ +# SuiteSparse Integration + +This directory contains CMake files for downloading and building external dependencies, including SuiteSparse. + +## SuiteSparse + +The `SuiteSparse.cmake` file handles the automatic download, build, and configuration of SuiteSparse from the official GitHub repository. + +### Components Built + +The following SuiteSparse components are built: +- **SuiteSparse_config**: Core configuration +- **AMD**: Approximate Minimum Degree ordering +- **COLAMD**: Column Approximate Minimum Degree ordering +- **CAMD**: Constrained Approximate Minimum Degree ordering +- **CCOLAMD**: Constrained Column Approximate Minimum Degree ordering +- **CHOLMOD**: Sparse Cholesky factorization +- **SPQR**: Sparse QR factorization + +### CMake Integration + +The SuiteSparse library is fetched and built automatically when you configure the project: + +```bash +cd test +mkdir build +cd build +cmake .. +make +``` + +### Dependencies + +SuiteSparse requires: +- BLAS (Basic Linear Algebra Subprograms) +- LAPACK (Linear Algebra Package) + +These are typically available on most systems: +- **macOS**: Pre-installed as part of Accelerate framework +- **Linux**: Install via `libblas-dev` and `liblapack-dev` packages + +The CMake build system will automatically find these dependencies. + +### Commit Version + +The integration uses SuiteSparse commit: `d558c83006d63d1dc62004f30042b3ca484f3f94` + +This ensures a stable, tested version of the library. + +### Build Output + +After building, SuiteSparse libraries are installed to: +``` +external/_deps/suitesparse-build/install/ +├── include/ +│ └── suitesparse/ +└── lib/ + ├── libspqr.a + ├── libcholmod.a + ├── libamd.a + ├── libcolamd.a + ├── libcamd.a + ├── libccolamd.a + └── libsuitesparseconfig.a +``` diff --git a/external/cmake-files/SuiteSparse.cmake b/external/cmake-files/SuiteSparse.cmake new file mode 100644 index 000000000..74828530f --- /dev/null +++ b/external/cmake-files/SuiteSparse.cmake @@ -0,0 +1,150 @@ +set(SUITESPARSE_CMAKE_DIR ${CMAKE_CURRENT_LIST_DIR}) +function(GetSuiteSparse) + find_path(SUITESPARSE_DIR NAMES include/SuiteSparse_config.h PATHS ${SUITESPARSE_CMAKE_DIR}/../_deps/suitesparse-build/install) + + if (NOT SUITESPARSE_DIR) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR "${SUITESPARSE_CMAKE_DIR}/../_deps") + + FetchContent_Declare( + suitesparse + GIT_REPOSITORY https://github.com/DrTimothyAldenDavis/SuiteSparse.git + GIT_TAG d558c83006d63d1dc62004f30042b3ca484f3f94 + ) + + FetchContent_GetProperties(suitesparse) + + if(NOT suitesparse_POPULATED) + message(STATUS "SuiteSparse library not found locally, downloading it.") + FetchContent_Populate(suitesparse) + + set(SUITESPARSE_SOURCE_DIR ${suitesparse_SOURCE_DIR}) + set(SUITESPARSE_BUILD_DIR ${suitesparse_BINARY_DIR}) + set(SUITESPARSE_INSTALL_DIR ${SUITESPARSE_BUILD_DIR}/install) + + # Create build directory + file(MAKE_DIRECTORY ${SUITESPARSE_BUILD_DIR}) + + # Configure SuiteSparse with CMake + # Build only the necessary components: SuiteSparse_config, AMD, COLAMD, CAMD, CCOLAMD, CHOLMOD, and SPQR + message(STATUS "Configuring SuiteSparse...") + execute_process( + COMMAND ${CMAKE_COMMAND} + -S ${SUITESPARSE_SOURCE_DIR} + -B ${SUITESPARSE_BUILD_DIR} + -DCMAKE_INSTALL_PREFIX=${SUITESPARSE_INSTALL_DIR} + -DCMAKE_BUILD_TYPE=Release + -DBUILD_SHARED_LIBS=OFF + -DBUILD_STATIC_LIBS=ON + -DSUITESPARSE_ENABLE_PROJECTS="suitesparse_config;amd;colamd;camd;ccolamd;cholmod;spqr" + -DCHOLMOD_CAMD=ON + -DCHOLMOD_SUPERNODAL=ON + WORKING_DIRECTORY ${SUITESPARSE_BUILD_DIR} + RESULT_VARIABLE SUITESPARSE_CONFIGURE_RESULT + OUTPUT_VARIABLE SUITESPARSE_CONFIGURE_OUTPUT + ERROR_VARIABLE SUITESPARSE_CONFIGURE_ERROR + ) + + if(NOT SUITESPARSE_CONFIGURE_RESULT EQUAL 0) + message(FATAL_ERROR "SuiteSparse configuration failed: ${SUITESPARSE_CONFIGURE_ERROR}") + endif() + + # Build SuiteSparse + message(STATUS "Building SuiteSparse...") + execute_process( + COMMAND ${CMAKE_COMMAND} --build ${SUITESPARSE_BUILD_DIR} --config Release + WORKING_DIRECTORY ${SUITESPARSE_BUILD_DIR} + RESULT_VARIABLE SUITESPARSE_BUILD_RESULT + OUTPUT_VARIABLE SUITESPARSE_BUILD_OUTPUT + ERROR_VARIABLE SUITESPARSE_BUILD_ERROR + ) + + if(NOT SUITESPARSE_BUILD_RESULT EQUAL 0) + message(FATAL_ERROR "SuiteSparse build failed: ${SUITESPARSE_BUILD_ERROR}") + endif() + + # Install SuiteSparse to the install directory + message(STATUS "Installing SuiteSparse...") + execute_process( + COMMAND ${CMAKE_COMMAND} --install ${SUITESPARSE_BUILD_DIR} --config Release + WORKING_DIRECTORY ${SUITESPARSE_BUILD_DIR} + RESULT_VARIABLE SUITESPARSE_INSTALL_RESULT + OUTPUT_VARIABLE SUITESPARSE_INSTALL_OUTPUT + ERROR_VARIABLE SUITESPARSE_INSTALL_ERROR + ) + + if(NOT SUITESPARSE_INSTALL_RESULT EQUAL 0) + message(FATAL_ERROR "SuiteSparse installation failed: ${SUITESPARSE_INSTALL_ERROR}") + endif() + + set(SUITESPARSE_DIR ${SUITESPARSE_INSTALL_DIR}) + message(STATUS "SuiteSparse built and installed at: ${SUITESPARSE_DIR}") + + endif() + else() + message(STATUS "SuiteSparse library found: ${SUITESPARSE_DIR}") + endif() + + # Set include directories + include_directories(BEFORE ${SUITESPARSE_DIR}/include) + include_directories(BEFORE ${SUITESPARSE_DIR}/include/suitesparse) + + # Set library directory + set(SUITESPARSE_LIB_DIR ${SUITESPARSE_DIR}/lib PARENT_SCOPE) + + # Find the libraries (check for both static and dynamic versions) + find_library(SPQR_LIB NAMES libspqr.a libspqr.dylib spqr PATHS ${SUITESPARSE_DIR}/lib NO_DEFAULT_PATH) + find_library(CHOLMOD_LIB NAMES libcholmod.a libcholmod.dylib cholmod PATHS ${SUITESPARSE_DIR}/lib NO_DEFAULT_PATH) + find_library(AMD_LIB NAMES libamd.a libamd.dylib amd PATHS ${SUITESPARSE_DIR}/lib NO_DEFAULT_PATH) + find_library(COLAMD_LIB NAMES libcolamd.a libcolamd.dylib colamd PATHS ${SUITESPARSE_DIR}/lib NO_DEFAULT_PATH) + find_library(CAMD_LIB NAMES libcamd.a libcamd.dylib camd PATHS ${SUITESPARSE_DIR}/lib NO_DEFAULT_PATH) + find_library(CCOLAMD_LIB NAMES libccolamd.a libccolamd.dylib ccolamd PATHS ${SUITESPARSE_DIR}/lib NO_DEFAULT_PATH) + find_library(SUITESPARSECONFIG_LIB NAMES libsuitesparseconfig.a libsuitesparseconfig.dylib suitesparseconfig PATHS ${SUITESPARSE_DIR}/lib NO_DEFAULT_PATH) + + # Verify all libraries were found + if(NOT SPQR_LIB OR NOT CHOLMOD_LIB OR NOT AMD_LIB OR NOT COLAMD_LIB OR NOT CAMD_LIB OR NOT CCOLAMD_LIB OR NOT SUITESPARSECONFIG_LIB) + message(FATAL_ERROR "Failed to find one or more SuiteSparse libraries in ${SUITESPARSE_DIR}/lib") + else() + message(STATUS "Found SPQR: ${SPQR_LIB}") + message(STATUS "Found CHOLMOD: ${CHOLMOD_LIB}") + message(STATUS "Found AMD: ${AMD_LIB}") + message(STATUS "Found COLAMD: ${COLAMD_LIB}") + message(STATUS "Found CAMD: ${CAMD_LIB}") + message(STATUS "Found CCOLAMD: ${CCOLAMD_LIB}") + message(STATUS "Found SuiteSparseConfig: ${SUITESPARSECONFIG_LIB}") + endif() + + # Create an interface library for SuiteSparse + if(NOT TARGET SuiteSparse::SPQR) + add_library(SuiteSparse::SPQR INTERFACE IMPORTED GLOBAL) + target_link_libraries(SuiteSparse::SPQR INTERFACE + ${SPQR_LIB} + ${CHOLMOD_LIB} + ${AMD_LIB} + ${COLAMD_LIB} + ${CAMD_LIB} + ${CCOLAMD_LIB} + ${SUITESPARSECONFIG_LIB} + ) + target_include_directories(SuiteSparse::SPQR INTERFACE + ${SUITESPARSE_DIR}/include + ${SUITESPARSE_DIR}/include/suitesparse + ) + endif() + + # Export the library list for direct use + set(SUITESPARSE_LIBRARIES + ${SPQR_LIB} + ${CHOLMOD_LIB} + ${AMD_LIB} + ${COLAMD_LIB} + ${CAMD_LIB} + ${CCOLAMD_LIB} + ${SUITESPARSECONFIG_LIB} + PARENT_SCOPE + ) + + message(STATUS "SuiteSparse libraries configured successfully") + +endfunction() + diff --git a/include/preprocess/full_dimensional_polytope.hpp b/include/preprocess/full_dimensional_polytope.hpp new file mode 100644 index 000000000..739fac856 --- /dev/null +++ b/include/preprocess/full_dimensional_polytope.hpp @@ -0,0 +1,183 @@ +#ifndef FULL_DIMENSIONAL_POLYTOPE_HPP +#define FULL_DIMENSIONAL_POLYTOPE_HPP + +#include +#include +#include +#include + +#include "SuiteSparseQR_C.h" +#include "SuiteSparseQR.hpp" + + +/** + * Compute the full dimensional polytope P' = {x | A'x <= b'} from the polytope P = {x | Ax <= b & Aeq*x = beq} + * + * @tparam NT Number type (default: double) + * @tparam SpMT Sparse matrix type (default: Eigen::SparseMatrix) + * @tparam MT Dense matrix type (default: Eigen::Matrix) + * @tparam VT Vector type (default: Eigen::Matrix) + * @param Aeq Sparse matrix of equality constraints (m x n) + * @param beq Right-hand side vector for equality constraints (m x 1) + * @param A Dense matrix of inequality constraints (p x n) + * @param b Right-hand side vector for inequality constraints (p x 1) + * + * @return std::tuple containing: + * - A_full: Transformed inequality matrix (p x (n-rank)) + * - b_full: Transformed inequality vector (p x 1) + * - shift: Translation vector (n x 1) + * - N: Nullspace transformation matrix (n x (n-rank)) + */ +template, + typename MT = Eigen::Matrix, + typename VT = Eigen::Matrix> +std::tuple compute_full_dimensional_polytope( + const SpMT& Aeq, + const VT& beq, + const MT& A, + const VT& b) +{ + cholmod_common c; + memset(&c, 0, sizeof(cholmod_common)); // Zero-initialize the structure + + // First try solving Aeq*x = beq using Eigen's sparse QR + Eigen::SparseQR> Aqr; + Aqr.compute(Aeq); + VT shift = Aqr.solve(beq); + + // If eigen fails try with SuiteSparse + if ((Aeq * shift - beq).cwiseAbs().maxCoeff() > 1e-09) + { + // Eigen failed to solve Aeq*x = beq + // Allocate the Aeq + cholmod_triplet *T = cholmod_allocate_triplet(Aeq.rows(), Aeq.cols(), Aeq.nonZeros(), 0, CHOLMOD_REAL, &c); + int i = 0; + for (int k=0; ki)[i] = it.row(); + ((int*)T->j)[i] = it.col(); + ((double*)T->x)[i] = it.value(); + i++; + } + } + T->nnz = Aeq.nonZeros(); + + // Convert triplet form to sparse matrix + cholmod_sparse *Aeq_chol = cholmod_triplet_to_sparse(T, Aeq.nonZeros(), &c); + // Create right-hand side vector beq for the CHOLMOD + cholmod_dense *b_chol = cholmod_allocate_dense(Aeq.rows(), 1, Aeq.rows(), CHOLMOD_REAL, &c); + double *b_values = (double *)b_chol->x; + for (int j = 0; j < beq.size(); j++) + { + b_values[j] = beq.coeff(j); + } + // Solve the linear system Aeq * x = beq using SuiteSparseQR_C_backslash + cholmod_dense *x = SuiteSparseQR_C_backslash(0, -4.0, Aeq_chol, b_chol, &c); + + if (!x) { + cholmod_free_dense(&x, &c); + cholmod_free_dense(&b_chol, &c); + cholmod_free_triplet(&T, &c); + cholmod_free_sparse(&Aeq_chol, &c); + cholmod_finish(&c); + throw std::runtime_error("Failed to solve the linear system."); + } + + // copy the solution x + shift.resize(Aeq.cols()); + NT *shift_data = shift.data(); + for (int j = 0; j < x->nrow; ++j) { + *shift_data = static_cast(((double*)x->x)[j]); + shift_data++; + } + + cholmod_free_dense(&x, &c); + cholmod_free_dense(&b_chol, &c); + cholmod_free_triplet(&T, &c); + cholmod_free_sparse(&Aeq_chol, &c); + } + + // Shift the polytope so that Aeq * x = 0 to hold for the feasible points + VT b_full = b - A * shift; + + // Allocate the Aeq.transpose() + cholmod_triplet *T = cholmod_allocate_triplet(Aeq.cols(), Aeq.rows(), Aeq.nonZeros(), 0, CHOLMOD_REAL, &c); + int i = 0; + for (int k=0; ki)[i] = it.col(); + ((int*)T->j)[i] = it.row(); + ((double*)T->x)[i] = it.value(); + i++; + } + } + T->nnz = Aeq.nonZeros(); // Set the number of non-zero elements + + // Convert triplet form to sparse matrix + cholmod_sparse *Aeq_tr = cholmod_triplet_to_sparse(T, Aeq.nonZeros(), &c); + int ordering = 0; // No permutation, natural order + int64_t *E = NULL; + int64_t econ = 0; // Use the rank of Aeq_tr + double tol = -4.0; // default tolerance + + // Call SuiteSparseQR_C_factorize + SuiteSparseQR_C_factorization *QR = SuiteSparseQR_C_factorize(ordering, tol, Aeq_tr, &c); + + if (!QR) { + cholmod_free_sparse(&Aeq_tr, &c); + cholmod_free_triplet(&T, &c); + SuiteSparseQR_C_free(&QR, &c); + cholmod_finish(&c); + throw std::runtime_error("QR factorization failed."); + } + + int rank = c.SPQR_istat[4]; // rank of Aeq.transpose() + + // Form the identity matrix + cholmod_dense *I = cholmod_zeros(Aeq_tr->nrow, Aeq_tr->nrow, CHOLMOD_REAL, &c); + for (int j = 0; j < Aeq_tr->nrow; j++) { + ((double*)I->x)[j * Aeq_tr->nrow + j] = 1.0; // Set diagonal elements to 1 + } + + // Output matrix to hold Q + cholmod_dense *Q = NULL; + + // Retrieve Q by multiplying Q with the identity matrix I + Q = SuiteSparseQR_C_qmult(1, // 1 for Q * I (to get Q) + QR, I, &c); + if (!Q) { + cholmod_free_sparse(&Aeq_tr, &c); + cholmod_free_dense(&Q, &c); + cholmod_free_dense(&I, &c); + cholmod_free_triplet(&T, &c); + SuiteSparseQR_C_free(&QR, &c); + cholmod_finish(&c); + throw std::runtime_error("Failed to retrieve Q matrix."); + } + int Q_rows = Q->nrow; + int Q_cols = Q->ncol; + double* Q_data = (double*)Q->x; + + // Map the data from CHOLMOD's Q to Eigen Matrix + Eigen::Map QQ(Q_data, Q_rows, Q_cols); + // Take the last n-r columns of Q to derive the right nullspace of Aeq.transpose() + MT N = QQ.block(0, rank, QQ.rows(), QQ.cols() - rank); + // Project the polytope to the null space + MT A_full = A * N; + + cholmod_free_sparse(&Aeq_tr, &c); + cholmod_free_dense(&Q, &c); + cholmod_free_dense(&I, &c); + cholmod_free_triplet(&T, &c); + SuiteSparseQR_C_free(&QR, &c); + cholmod_finish(&c); + + return std::make_tuple(A_full, b_full, shift, N); +} + +#endif // FULL_DIMENSIONAL_POLYTOPE_HPP diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 074a83d90..bb1f68f30 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -122,6 +122,9 @@ GetLPSolve() include("../external/cmake-files/QD.cmake") GetQD() +include("../external/cmake-files/SuiteSparse.cmake") +GetSuiteSparse() + # Code Coverage Configuration add_library(coverage_config INTERFACE) @@ -380,6 +383,26 @@ add_test(NAME test_volumetric_center add_test(NAME test_vaidya_center COMMAND test_internal_points -tc=test_vaidya_center) +add_executable (full_dimensional_polytope_test full_dimensional_polytope_test.cpp $) +add_test(NAME full_dimensional_polytope_canonical_simplex + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_canonical_simplex) +add_test(NAME full_dimensional_polytope_simple_feasible + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_simple_feasible) +add_test(NAME full_dimensional_polytope_line_segment + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_line_segment) +add_test(NAME full_dimensional_polytope_high_dimensional + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_high_dimensional) +add_test(NAME full_dimensional_polytope_multiple_constraints + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_multiple_constraints) +add_test(NAME full_dimensional_polytope_hypercube_intersection + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_hypercube_intersection) +add_test(NAME full_dimensional_polytope_sparse_constraint + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_sparse_constraint) +add_test(NAME full_dimensional_polytope_orthogonality + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_orthogonality) +add_test(NAME full_dimensional_polytope_infeasible + COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_infeasible) + set(ADDITIONAL_FLAGS "-march=native -DSIMD_LEN=0 -DTIME_KEEPING") @@ -424,3 +447,4 @@ TARGET_LINK_LIBRARIES(crhmc_sampling_test lp_solve ${IFOPT} ${IFOPT_IPOPT} ${PTH TARGET_LINK_LIBRARIES(order_polytope lp_solve coverage_config) TARGET_LINK_LIBRARIES(matrix_sampling_test lp_solve ${MKL_LINK} coverage_config) TARGET_LINK_LIBRARIES(test_internal_points lp_solve ${MKL_LINK} coverage_config) +TARGET_LINK_LIBRARIES(full_dimensional_polytope_test ${SUITESPARSE_LIBRARIES} ${MKL_LINK} coverage_config) diff --git a/test/full_dimensional_polytope_test.cpp b/test/full_dimensional_polytope_test.cpp new file mode 100644 index 000000000..76b181279 --- /dev/null +++ b/test/full_dimensional_polytope_test.cpp @@ -0,0 +1,574 @@ +// VolEsti (volume computation and sampling library) + +// Copyright (c) 2012-2025 Vissarion Fisikopoulos +// Copyright (c) 2018-2025 Apostolos Chalkis + +// Licensed under GNU LGPL.3, see LICENCE file + +#include "doctest.h" +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "preprocess/full_dimensional_polytope.hpp" +#include "preprocess/feasible_point.hpp" +#include "convex_bodies/hpolytope.h" +#include "cartesian_geom/cartesian_kernel.h" +#include "random_walks/random_walks.hpp" +#include "sampling/sampling.hpp" + +typedef double NT; +typedef Eigen::Matrix MT; +typedef Eigen::Matrix VT; +typedef Eigen::SparseMatrix SpMT; +typedef Cartesian Kernel; +typedef typename Kernel::Point Point; +typedef HPolytope Hpolytope; +typedef BoostRandomNumberGenerator RNGType; + +// Helper function to check if a point satisfies Ax <= b +bool satisfies_inequality(const MT& A, const VT& b, const VT& x, NT tol = 1e-6) +{ + VT residual = A * x - b; + return (residual.array() <= tol).all(); +} + +// Helper function to check if a point satisfies Aeq*x = beq +bool satisfies_equality(const SpMT& Aeq, const VT& beq, const VT& x, NT tol = 1e-6) +{ + VT residual = Aeq * x - beq; + return residual.cwiseAbs().maxCoeff() < tol; +} + +// Helper function to create sparse matrix from dense +SpMT dense_to_sparse(const MT& dense) +{ + SpMT sparse = dense.sparseView(); + sparse.makeCompressed(); + return sparse; +} + +// Helper function to sample from a polytope using AcceleratedBilliardWalk +std::vector sample_from_polytope(const MT& A, const VT& b, unsigned int num_samples = 100) +{ + unsigned int d = A.cols(); + + // Create H-polytope + Hpolytope P(d, A, b); + + // Compute inner ball and get its center as starting point + std::pair inner_ball = P.ComputeInnerBall(); + Point StartingPoint = inner_ball.first; + + // Setup sampling + RNGType rng(d); + unsigned int walkL = 1, nburns = 0; + std::list randPoints; + + // Sample using AcceleratedBilliardWalk + uniform_sampling(randPoints, P, rng, walkL, num_samples, StartingPoint, nburns); + + // Convert to vector of VT + std::vector samples; + samples.reserve(num_samples); + for (const auto& pt : randPoints) + { + samples.push_back(pt.getCoefficients()); + } + + return samples; +} + +TEST_CASE("full_dimensional_polytope_canonical_simplex") +{ + std::cout << "\n=== Test: Canonical simplex (n-1 dimensional in R^n) ===" << std::endl; + + // Canonical simplex in R^3: x1 + x2 + x3 = 1, x1, x2, x3 >= 0 + // This is a 2D simplex embedded in 3D + int n = 3; + + // Equality constraint: x1 + x2 + x3 = 1 + MT Aeq_dense(1, n); + Aeq_dense << 1, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 1; + + // Inequality constraints: x1, x2, x3 >= 0 + MT A(n, n); + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1; + + VT b = VT::Zero(n); + + // Compute full dimensional polytope + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Original dimension: " << n << std::endl; + std::cout << "Reduced dimension: " << N.cols() << std::endl; + std::cout << "Rank of Aeq: " << Aeq_dense.rows() << std::endl; + std::cout << "Shift: " << shift.transpose() << std::endl; + + // Verify dimension reduction + CHECK(N.cols() == n - 1); // Should reduce to 2D + CHECK(N.rows() == n); + + // Verify that N is in the nullspace of Aeq + MT Aeq_N = Aeq_dense * N; + CHECK(Aeq_N.cwiseAbs().maxCoeff() < 1e-6); + + // Verify that the transformation preserves the polytope structure + CHECK(A_full.rows() == A.rows()); + CHECK(A_full.cols() == N.cols()); + + // Check that the output polytope is feasible + VT x_feasible = compute_feasible_point(A_full, b_full); + CHECK(satisfies_inequality(A_full, b_full, x_feasible, 1e-6)); + std::cout << "Output polytope is feasible" << std::endl; + + // Sample from the output (full-dimensional) polytope + std::cout << "Sampling from output polytope..." << std::endl; + std::vector samples = sample_from_polytope(A_full, b_full, 50); + + // Map sampled points back to original space and verify constraints + int num_valid = 0; + for (const auto& x_reduced : samples) + { + // Map to original space: y = shift + N * x + VT y_original = shift + N * x_reduced; + + // Check equality constraints + bool eq_satisfied = satisfies_equality(Aeq, beq, y_original, 1e-5); + + // Check inequality constraints + bool ineq_satisfied = satisfies_inequality(A, b, y_original, 1e-5); + + if (eq_satisfied && ineq_satisfied) + num_valid++; + } + + std::cout << "Valid samples: " << num_valid << " / " << samples.size() << std::endl; + CHECK(num_valid == samples.size()); +} + +TEST_CASE("full_dimensional_polytope_simple_feasible") +{ + std::cout << "\n=== Test: Simple feasible case (2D in 3D) ===" << std::endl; + + // 2D plane in 3D: x + y + z = 3, with bounds 0 <= x,y,z <= 2 + int n = 3; + + // Equality constraint: x + y + z = 3 + MT Aeq_dense(1, n); + Aeq_dense << 1, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 3; + + // Inequality constraints: 0 <= x,y,z <= 2 + MT A(6, n); + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1; + + VT b(6); + b << 0, 0, 0, 2, 2, 2; + + // Compute full dimensional polytope + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Original dimension: " << n << std::endl; + std::cout << "Reduced dimension: " << N.cols() << std::endl; + std::cout << "Shift: " << shift.transpose() << std::endl; + + // Verify dimension reduction + CHECK(N.cols() == n - 1); + + // Verify N is in nullspace + MT Aeq_N = Aeq_dense * N; + CHECK(Aeq_N.cwiseAbs().maxCoeff() < 1e-6); + + // Check output polytope feasibility + VT x_feasible = compute_feasible_point(A_full, b_full); + CHECK(satisfies_inequality(A_full, b_full, x_feasible, 1e-6)); + + // Sample and verify transformation + std::vector samples = sample_from_polytope(A_full, b_full, 50); + int num_valid = 0; + for (const auto& x_reduced : samples) + { + VT y_original = shift + N * x_reduced; + if (satisfies_equality(Aeq, beq, y_original, 1e-5) && + satisfies_inequality(A, b, y_original, 1e-5)) + num_valid++; + } + std::cout << "Valid samples: " << num_valid << " / " << samples.size() << std::endl; + CHECK(num_valid == samples.size()); +} + +TEST_CASE("full_dimensional_polytope_line_segment") +{ + std::cout << "\n=== Test: Line segment (1D in 2D) ===" << std::endl; + + int n = 2; + + MT Aeq_dense(1, n); + Aeq_dense << 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 1; + + MT A(4, n); + A << -1, 0, + 1, 0, + 0, -1, + 0, 1; + + VT b(4); + b << 0, 1, 0, 1; + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Original dimension: " << n << std::endl; + std::cout << "Reduced dimension: " << N.cols() << std::endl; + + CHECK(N.cols() == 1); + + MT Aeq_N = Aeq_dense * N; + CHECK(Aeq_N.cwiseAbs().maxCoeff() < 1e-6); + + VT x_feasible = compute_feasible_point(A_full, b_full); + CHECK(satisfies_inequality(A_full, b_full, x_feasible, 1e-6)); + + std::vector samples = sample_from_polytope(A_full, b_full, 50); + int num_valid = 0; + for (const auto& x_reduced : samples) + { + VT y_original = shift + N * x_reduced; + if (satisfies_equality(Aeq, beq, y_original, 1e-5) && + satisfies_inequality(A, b, y_original, 1e-5)) + num_valid++; + } + std::cout << "Valid samples: " << num_valid << " / " << samples.size() << std::endl; + CHECK(num_valid == samples.size()); +} + +TEST_CASE("full_dimensional_polytope_high_dimensional") +{ + std::cout << "\n=== Test: High dimensional (8D in 10D) ===" << std::endl; + + int n = 10; + int m = 2; + + MT Aeq_dense(m, n); + Aeq_dense << 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 1, 1, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(m); + beq << 5, 5; + + MT A = -MT::Identity(n, n); + VT b = VT::Zero(n); + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Original dimension: " << n << std::endl; + std::cout << "Reduced dimension: " << N.cols() << std::endl; + + CHECK(N.cols() == n - m); + + MT Aeq_N = Aeq_dense * N; + CHECK(Aeq_N.cwiseAbs().maxCoeff() < 1e-6); + + VT x_feasible = compute_feasible_point(A_full, b_full); + CHECK(satisfies_inequality(A_full, b_full, x_feasible, 1e-6)); + + // For high dimensional, test with fewer samples + std::vector samples = sample_from_polytope(A_full, b_full, 30); + int num_valid = 0; + for (const auto& x_reduced : samples) + { + VT y_original = shift + N * x_reduced; + if (satisfies_equality(Aeq, beq, y_original, 1e-5) && + satisfies_inequality(A, b, y_original, 1e-5)) + num_valid++; + } + std::cout << "Valid samples: " << num_valid << " / " << samples.size() << std::endl; + CHECK(num_valid == samples.size()); +} + +TEST_CASE("full_dimensional_polytope_multiple_constraints") +{ + std::cout << "\n=== Test: Multiple equality constraints (3D to 1D) ===" << std::endl; + + int n = 3; + int m = 2; + + MT Aeq_dense(m, n); + Aeq_dense << 1, 1, 0, + 0, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(m); + beq << 1, 1; + + MT A(6, n); + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1; + + VT b(6); + b << 0, 0, 0, 1, 1, 1; + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Original dimension: " << n << std::endl; + std::cout << "Reduced dimension: " << N.cols() << std::endl; + + CHECK(N.cols() == n - m); + CHECK(N.cols() == 1); + + MT Aeq_N = Aeq_dense * N; + CHECK(Aeq_N.cwiseAbs().maxCoeff() < 1e-6); + + VT x_feasible = compute_feasible_point(A_full, b_full); + CHECK(satisfies_inequality(A_full, b_full, x_feasible, 1e-6)); + + std::vector samples = sample_from_polytope(A_full, b_full, 50); + int num_valid = 0; + for (const auto& x_reduced : samples) + { + VT y_original = shift + N * x_reduced; + if (satisfies_equality(Aeq, beq, y_original, 1e-5) && + satisfies_inequality(A, b, y_original, 1e-5)) + num_valid++; + } + std::cout << "Valid samples: " << num_valid << " / " << samples.size() << std::endl; + CHECK(num_valid == samples.size()); +} + +TEST_CASE("full_dimensional_polytope_hypercube_intersection") +{ + std::cout << "\n=== Test: Hypercube with plane (3D to 2D) ===" << std::endl; + + int n = 3; + + MT Aeq_dense(1, n); + Aeq_dense << 1, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 1.5; + + MT A(6, n); + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1; + + VT b(6); + b << 0, 0, 0, 1, 1, 1; + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Original dimension: " << n << std::endl; + std::cout << "Reduced dimension: " << N.cols() << std::endl; + + CHECK(N.cols() == 2); + + MT Aeq_N = Aeq_dense * N; + CHECK(Aeq_N.cwiseAbs().maxCoeff() < 1e-6); + + VT x_feasible = compute_feasible_point(A_full, b_full); + CHECK(satisfies_inequality(A_full, b_full, x_feasible, 1e-6)); + + std::vector samples = sample_from_polytope(A_full, b_full, 50); + int num_valid = 0; + for (const auto& x_reduced : samples) + { + VT y_original = shift + N * x_reduced; + if (satisfies_equality(Aeq, beq, y_original, 1e-5) && + satisfies_inequality(A, b, y_original, 1e-5)) + num_valid++; + } + std::cout << "Valid samples: " << num_valid << " / " << samples.size() << std::endl; + CHECK(num_valid == samples.size()); +} + +TEST_CASE("full_dimensional_polytope_sparse_constraint") +{ + std::cout << "\n=== Test: Sparse equality constraint (5D to 4D) ===" << std::endl; + + int n = 5; + + MT Aeq_dense = MT::Zero(1, n); + Aeq_dense(0, 0) = 1; + Aeq_dense(0, 4) = 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 2; + + MT A(2*n, n); + A.topRows(n) = -MT::Identity(n, n); + A.bottomRows(n) = MT::Identity(n, n); + + VT b(2*n); + b.head(n).setZero(); + b.tail(n).setConstant(2); + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Original dimension: " << n << std::endl; + std::cout << "Reduced dimension: " << N.cols() << std::endl; + + CHECK(N.cols() == 4); + + MT Aeq_N = Aeq_dense * N; + CHECK(Aeq_N.cwiseAbs().maxCoeff() < 1e-6); + + VT x_feasible = compute_feasible_point(A_full, b_full); + CHECK(satisfies_inequality(A_full, b_full, x_feasible, 1e-6)); + + std::vector samples = sample_from_polytope(A_full, b_full, 50); + int num_valid = 0; + for (const auto& x_reduced : samples) + { + VT y_original = shift + N * x_reduced; + if (satisfies_equality(Aeq, beq, y_original, 1e-5) && + satisfies_inequality(A, b, y_original, 1e-5)) + num_valid++; + } + std::cout << "Valid samples: " << num_valid << " / " << samples.size() << std::endl; + CHECK(num_valid == samples.size()); +} + +TEST_CASE("full_dimensional_polytope_orthogonality") +{ + std::cout << "\n=== Test: Nullspace orthogonality properties ===" << std::endl; + + int n = 4; + + MT Aeq_dense(1, n); + Aeq_dense << 1, 1, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 4; + + MT A = -MT::Identity(n, n); + VT b = VT::Zero(n); + + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Original dimension: " << n << std::endl; + std::cout << "Reduced dimension: " << N.cols() << std::endl; + + MT Aeq_N = Aeq_dense * N; + CHECK(Aeq_N.cwiseAbs().maxCoeff() < 1e-6); + + MT NtN = N.transpose() * N; + MT I = MT::Identity(N.cols(), N.cols()); + CHECK((NtN - I).cwiseAbs().maxCoeff() < 1e-6); + + std::cout << "N^T * N =\n" << NtN << std::endl; + std::cout << "Columns are orthonormal: " + << ((NtN - I).cwiseAbs().maxCoeff() < 1e-6 ? "YES" : "NO") << std::endl; + + // Also verify with sampling + std::vector samples = sample_from_polytope(A_full, b_full, 30); + int num_valid = 0; + for (const auto& x_reduced : samples) + { + VT y_original = shift + N * x_reduced; + if (satisfies_equality(Aeq, beq, y_original, 1e-5) && + satisfies_inequality(A, b, y_original, 1e-5)) + num_valid++; + } + std::cout << "Valid samples: " << num_valid << " / " << samples.size() << std::endl; + CHECK(num_valid == samples.size()); +} + +TEST_CASE("full_dimensional_polytope_infeasible") +{ + std::cout << "\n=== Test: Infeasible system (plane outside cube) ===" << std::endl; + + int n = 3; + + MT Aeq_dense(1, n); + Aeq_dense << 1, 1, 1; + SpMT Aeq = dense_to_sparse(Aeq_dense); + + VT beq(1); + beq << 5; // Impossible since max(x+y+z) = 3 in unit cube + + MT A(6, n); + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1; + + VT b(6); + b << 0, 0, 0, 1, 1, 1; + + // This should still produce output, but the output polytope should be infeasible + try { + auto [A_full, b_full, shift, N] = compute_full_dimensional_polytope(Aeq, beq, A, b); + + std::cout << "Shift found: " << shift.transpose() << std::endl; + + // Check if equality is satisfied by shift (it should be) + CHECK(satisfies_equality(Aeq, beq, shift, 1e-6)); + std::cout << "Equality constraint satisfied by shift: YES" << std::endl; + + // Shift should NOT satisfy the inequalities (problem is infeasible) + bool shift_feasible = satisfies_inequality(A, b, shift, 1e-6); + std::cout << "Shift satisfies inequalities: " << (shift_feasible ? "YES" : "NO") << std::endl; + CHECK(!shift_feasible); // Should be infeasible + + // The output polytope A_full * x <= b_full should also be infeasible + std::cout << "Attempting to find feasible point in output polytope..." << std::endl; + bool output_polytope_feasible = true; + try { + VT x_feasible = compute_feasible_point(A_full, b_full); + // If we reach here, check if it actually satisfies the constraints + if (satisfies_inequality(A_full, b_full, x_feasible, 1e-6)) { + std::cout << "WARNING: Output polytope appears feasible!" << std::endl; + output_polytope_feasible = true; + } else { + output_polytope_feasible = false; + } + } catch (const std::exception& e) { + std::cout << "compute_feasible_point failed (expected): " << e.what() << std::endl; + output_polytope_feasible = false; + } + + // For infeasible input, output polytope should also be infeasible + std::cout << "Output polytope feasible: " << (output_polytope_feasible ? "YES" : "NO") << std::endl; + CHECK(!output_polytope_feasible); + + } catch (const std::exception& e) { + std::cout << "Exception during transformation: " << e.what() << std::endl; + } +}