From a81e68d6e2de229280836bb18d85375a10fa2c92 Mon Sep 17 00:00:00 2001 From: Simon Rohou Date: Sun, 15 Dec 2024 11:45:05 +0100 Subject: [PATCH] [ellips] updates (homogenization + Eigen optimizations) --- examples/ellipsoid_example/main.cpp | 87 +++++++++++-------- python/src/core/CMakeLists.txt | 1 + python/src/core/codac2_py_core.cpp | 2 + .../domains/ellipsoid/codac2_py_Ellipsoid.cpp | 23 +++-- .../ellipsoid/codac2_py_Ellipsoid_utils.cpp | 35 ++++++++ .../graphics/figures/codac2_py_Figure2D.cpp | 6 ++ .../domains/ellipsoid/codac2_Ellipsoid.cpp | 69 +++++++-------- src/core/domains/ellipsoid/codac2_Ellipsoid.h | 3 +- .../ellipsoid/codac2_Ellipsoid_utils.cpp | 53 ++++++----- .../ellipsoid/codac2_Ellipsoid_utils.h | 9 +- src/graphics/CMakeLists.txt | 1 - src/graphics/figures/codac2_Figure2D.cpp | 41 ++++++++- src/graphics/figures/codac2_Figure2D.h | 3 + .../figures/codac2_Figure2D_ellipsoid.cpp | 56 ------------ .../figures/codac2_OutputFigure2D.cpp | 4 +- 15 files changed, 226 insertions(+), 167 deletions(-) create mode 100644 python/src/core/domains/ellipsoid/codac2_py_Ellipsoid_utils.cpp delete mode 100644 src/graphics/figures/codac2_Figure2D_ellipsoid.cpp diff --git a/examples/ellipsoid_example/main.cpp b/examples/ellipsoid_example/main.cpp index 1c328c8e..c3cb3753 100644 --- a/examples/ellipsoid_example/main.cpp +++ b/examples/ellipsoid_example/main.cpp @@ -13,10 +13,11 @@ int main() { fig1.set_window_properties({0, 100}, {500, 500}); // initial ellipsoid - Vector mu({1., 0.}); - Matrix G({{0.05, 0.0}, - {0., 0.05}}); - Ellipsoid e1(mu, G); + Ellipsoid e1( + {1., 0.}, // mu + {{0.05, 0.0}, // G + {0., 0.05}} + ); fig1.draw_ellipsoid(e1, {Color::red(), Color::red(0.3)}); cout << "Initial ellipsoid e1 (red):" << endl; cout << e1 << endl; @@ -51,10 +52,10 @@ int main() { int Np = 200; for (int i = 0; i < Np; i++) { Vector x0 = e1.rand(); - fig1.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)}); + fig1.draw_point(x0, {Color::black(), Color::black(0.3)}); for (int j = 0; j < N; j++) { x0 = h.eval(x0).mid(); - fig1.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)}); + fig1.draw_point(x0, {Color::black(), Color::black(0.3)}); } } @@ -62,19 +63,24 @@ int main() { // ellipsoid projections // ---------------------------------------------------------- - Vector mu4({1., 0., 0.}); - Matrix G4({{1., 0.5, 0.}, - {0.5, 2., 0.2}, - {0., 0.2, 3.}}); - Ellipsoid e4(mu4, G4); - - Matrix G5 = 0.7 * G4; - Ellipsoid e5(mu4, G5); - - Matrix G6({{2., 0., 0.5}, - {0., 1., 0.2}, - {0., 0.2, 3.}}); - Ellipsoid e6(mu4, G6); + Ellipsoid e4 { + {1., 0., 0.}, // mu + {{1., 0.5, 0.}, // G + {0.5, 2., 0.2}, + {0., 0.2, 3.}} + }; + + Ellipsoid e5 { + e4.mu, // mu + 0.7 * e4.G // G + }; + + Ellipsoid e6 { + e4.mu, // mu + {{2., 0., 0.5}, // G + {0., 1., 0.2}, + {0., 0.2, 3.}} + }; Figure2D fig2("Projected ellipsoid xy", GraphicOutput::VIBES); Figure2D fig3("Projected ellipsoid yz", GraphicOutput::VIBES); @@ -102,11 +108,10 @@ int main() { // particle cloud (draw the evolution of 200 points in the ellipsoid e5) for (int i = 0; i < Np; i++) { - IntervalVector x5(e5.rand()); - x5.inflate(0.001); - fig2.draw_box(x5, {Color::black(), Color::black(0.3)}); - fig3.draw_box(x5, {Color::black(), Color::black(0.3)}); - fig4.draw_box(x5, {Color::black(), Color::black(0.3)}); + Vector x5 = e5.rand(); + fig2.draw_point(x5, {Color::black(), Color::black(0.3)}); + fig3.draw_point(x5, {Color::black(), Color::black(0.3)}); + fig4.draw_point(x5, {Color::black(), Color::black(0.3)}); } // ---------------------------------------------------------- @@ -149,10 +154,16 @@ int main() { fig5.set_axes(axis(0, {-0.5, 2}), axis(1, {-1.5, 1.})); fig5.set_window_properties({700, 600}, {500, 500}); - Ellipsoid e9(Vector({0., 0.5}), Matrix({{0.25, 0.}, - {0., 0.}})); - Ellipsoid e10(Vector({0., -0.5}), Matrix({{0.25, 0.}, - {0., 0.25}})); + Ellipsoid e9 { + {0., 0.5}, // mu + {{0.25, 0.}, // G + {0., 0.}} + }; + Ellipsoid e10 { + {0., -0.5}, // mu + {{0.25, 0.}, // G + {0., 0.25}} + }; fig5.draw_ellipsoid(e9, {Color::blue(), Color::red(0.3)}); fig5.draw_ellipsoid(e10, {Color::red(), Color::red(0.3)}); @@ -176,14 +187,12 @@ int main() { // particle cloud (draw the evolution of 200 points in the ellipsoid) for (int i = 0; i < Np; i++) { Vector x0 = e9.rand(); - fig5.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)}); - x0 = h2.eval(x0).mid(); - fig5.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)}); + fig5.draw_point(x0, {Color::black(), Color::black(0.3)}); + fig5.draw_point(h2.eval(x0).mid(), {Color::black(), Color::black(0.3)}); x0 = e10.rand(); - fig5.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)}); - x0 = h3.eval(x0).mid(); - fig5.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)}); + fig5.draw_point(x0, {Color::black(), Color::black(0.3)}); + fig5.draw_point(h3.eval(x0).mid(), {Color::black(), Color::black(0.3)}); } // ---------------------------------------------------------- @@ -193,8 +202,14 @@ int main() { // pendulum example AnalyticFunction h4{ {x}, vec(x[0] + 0.5 * x[1] , x[1] + 0.5 * (-x[1]-sin(x[0])))}; - Ellipsoid e13(Vector(2), Matrix(2,2)); - Ellipsoid e13_out(Vector(2), Matrix(2,2)); + Ellipsoid e13 { + Vector::zero(2), // mu + Matrix::zero(2,2) // G + }; + Ellipsoid e13_out { + Vector::zero(2), // mu + Matrix::zero(2,2) // G + }; int alpha_max = 1; if(stability_analysis(h4,alpha_max, e13, e13_out) == BoolInterval::TRUE) diff --git a/python/src/core/CMakeLists.txt b/python/src/core/CMakeLists.txt index c2d7a483..0bc86a5a 100644 --- a/python/src/core/CMakeLists.txt +++ b/python/src/core/CMakeLists.txt @@ -33,6 +33,7 @@ contractors/codac2_py_linear_ctc.cpp domains/ellipsoid/codac2_py_Ellipsoid.cpp + domains/ellipsoid/codac2_py_Ellipsoid_utils.cpp domains/interval/codac2_py_Interval.cpp domains/interval/codac2_py_Interval_operations.cpp domains/interval/codac2_py_IntervalMatrix.cpp diff --git a/python/src/core/codac2_py_core.cpp b/python/src/core/codac2_py_core.cpp index dbe2355c..89c071a9 100644 --- a/python/src/core/codac2_py_core.cpp +++ b/python/src/core/codac2_py_core.cpp @@ -53,6 +53,7 @@ void export_linear_ctc(py::module& m); // domains void export_BoolInterval(py::module& m); void export_Ellipsoid(py::module& m); +void export_Ellipsoid_utils(py::module& m); py::class_ export_Interval(py::module& m); void export_Interval_operations(py::module& m, py::class_& py_Interval); py::class_ export_IntervalRow(py::module& m); @@ -164,6 +165,7 @@ PYBIND11_MODULE(_core, m) // domains export_BoolInterval(m); export_Ellipsoid(m); + export_Ellipsoid_utils(m); auto py_Interval = export_Interval(m); export_Interval_operations(m, py_Interval); auto py_IR = export_IntervalRow(m); diff --git a/python/src/core/domains/ellipsoid/codac2_py_Ellipsoid.cpp b/python/src/core/domains/ellipsoid/codac2_py_Ellipsoid.cpp index a411561e..e1784a10 100644 --- a/python/src/core/domains/ellipsoid/codac2_py_Ellipsoid.cpp +++ b/python/src/core/domains/ellipsoid/codac2_py_Ellipsoid.cpp @@ -29,7 +29,7 @@ void export_Ellipsoid(py::module& m) .def_readwrite("G", &Ellipsoid::G, MATRIX_ELLIPSOID_G) - .def(py::init(), + .def(py::init(), ELLIPSOID_ELLIPSOID_INDEX, "n"_a) @@ -40,13 +40,6 @@ void export_Ellipsoid(py::module& m) .def(py::init(), "e"_a) - .def("__repr__", [](const Ellipsoid& e) { - std::ostringstream s; - s << e; - return string(s.str()); - }, - OSTREAM_REF_OPERATOROUT_OSTREAM_REF_CONST_ELLIPSOID_REF) - .def("size", &Ellipsoid::size, INDEX_ELLIPSOID_SIZE_CONST) @@ -60,8 +53,20 @@ void export_Ellipsoid(py::module& m) BOOLINTERVAL_ELLIPSOID_IS_CONCENTRIC_SUBSET_CONST_ELLIPSOID_REF_CONST, "e"_a) + .def("proj_2d", &Ellipsoid::proj_2d, + ELLIPSOID_ELLIPSOID_PROJ_2D_CONST_VECTOR_REF_CONST_VECTOR_REF_CONST_VECTOR_REF_CONST, + "d"_a, "v"_a, "u"_a) + + .def("__repr__", [](const Ellipsoid& e) { + std::ostringstream s; + s << e; + return string(s.str()); + }, + OSTREAM_REF_OPERATOROUT_OSTREAM_REF_CONST_ELLIPSOID_REF) + .def(py::self + py::self, - ELLIPSOID_OPERATORPLUS_CONST_ELLIPSOID_REF_CONST_ELLIPSOID_REF) + ELLIPSOID_OPERATORPLUS_CONST_ELLIPSOID_REF_CONST_ELLIPSOID_REF, + py::is_operator()) ; diff --git a/python/src/core/domains/ellipsoid/codac2_py_Ellipsoid_utils.cpp b/python/src/core/domains/ellipsoid/codac2_py_Ellipsoid_utils.cpp new file mode 100644 index 00000000..1fd51de1 --- /dev/null +++ b/python/src/core/domains/ellipsoid/codac2_py_Ellipsoid_utils.cpp @@ -0,0 +1,35 @@ +/** + * Codac binding (core) + * ---------------------------------------------------------------------------- + * \date 2024 + * \author Simon Rohou + * \copyright Copyright 2024 Codac Team + * \license GNU Lesser General Public License (LGPL) + */ + +#include +#include +#include +#include +#include +#include "codac2_py_Ellipsoid_utils_docs.h" // Generated file from Doxygen XML (doxygen2docstring.py): + +using namespace std; +using namespace codac2; +namespace py = pybind11; +using namespace pybind11::literals; + +void export_Ellipsoid_utils(py::module& m) +{ + m + + .def("stability_analysis", &codac2::stability_analysis, + BOOLINTERVAL_STABILITY_ANALYSIS_CONST_ANALYTICFUNCTION_VECTOROPVALUE_REF_UNSIGNED_INT_ELLIPSOID_REF_ELLIPSOID_REF_BOOL, + "f"_a, "alpha_max"_a, "e"_a, "e_out"_a, "verbose"_a=false) + + .def("solve_discrete_lyapunov", &codac2::solve_discrete_lyapunov, + MATRIX_SOLVE_DISCRETE_LYAPUNOV_CONST_MATRIX_REF_CONST_MATRIX_REF, + "A"_a, "Q"_a) + + ; +} \ No newline at end of file diff --git a/python/src/graphics/figures/codac2_py_Figure2D.cpp b/python/src/graphics/figures/codac2_py_Figure2D.cpp index 4a3e18d1..4c9e143f 100644 --- a/python/src/graphics/figures/codac2_py_Figure2D.cpp +++ b/python/src/graphics/figures/codac2_py_Figure2D.cpp @@ -65,6 +65,12 @@ void export_Figure2D(py::module& m) VOID_FIGURE2D_SET_AXES_CONST_FIGUREAXIS_REF_CONST_FIGUREAXIS_REF, "axis1"_a, "axis2"_a) + .def("i", &Figure2D::i, + CONST_INDEX_REF_FIGURE2D_I_CONST) + + .def("j", &Figure2D::j, + CONST_INDEX_REF_FIGURE2D_J_CONST) + .def("pos", &Figure2D::pos, CONST_VECTOR_REF_FIGURE2D_POS_CONST) diff --git a/src/core/domains/ellipsoid/codac2_Ellipsoid.cpp b/src/core/domains/ellipsoid/codac2_Ellipsoid.cpp index 8dd67f45..f7102e3a 100644 --- a/src/core/domains/ellipsoid/codac2_Ellipsoid.cpp +++ b/src/core/domains/ellipsoid/codac2_Ellipsoid.cpp @@ -17,7 +17,7 @@ using codac2::Vector; namespace codac2 { Ellipsoid::Ellipsoid(Index n) - : mu(Vector(n)), G(Matrix(n, n)) { + : mu(Vector::zero(n)), G(Matrix::zero(n, n)) { assert_release(n > 0); } @@ -27,6 +27,7 @@ namespace codac2 { } Index Ellipsoid::size() const { + assert(mu.size() == G.cols() && G.is_squared()); return mu.size(); } @@ -89,13 +90,13 @@ namespace codac2 { return BoolInterval::TRUE; } - void Ellipsoid::projection2D(const Vector& d, const Vector& v, const Vector& u) + Ellipsoid Ellipsoid::proj_2d(const Vector& d, const Vector& v, const Vector& u) const { // from [Pope S. B. - Algorithms for Ellipsoids - 2008] - assert( d.size() == v.size()); - assert( d.size() == u.size()); - assert( d.size() == this->size()); - assert( (v.transpose()*u).norm() == 0); // u & v orthogonal + assert_release( d.size() == v.size()); + assert_release( d.size() == u.size()); + assert_release( d.size() == this->size()); + assert_release( (v.transpose()*u).norm() == 0); // u & v orthogonal // Normalized Projection matrix // the plane (d,u,v) is also the affine plan {x|x=d+Tt} with T = [u,v] @@ -104,22 +105,19 @@ namespace codac2 { T.col(1) = u/u.norm(); auto TTG = T.transpose() * this->G; - Eigen::BDCSVD bdcsvd(TTG, Eigen::ComputeFullU); - Matrix U(bdcsvd.matrixU()); - Matrix E((Eigen::MatrixXd) bdcsvd.singularValues().asDiagonal()); - this->G = U * E; - this->mu = T.transpose() * (d + T * T.transpose() * (this->mu - d)); + Eigen::BDCSVD bdcsvd(TTG); + auto U = bdcsvd.matrixU(); + auto E = bdcsvd.singularValues().asDiagonal(); + + return { + T.transpose() * (d + T * T.transpose() * (this->mu - d)), + U * E + }; } Ellipsoid operator+(const Ellipsoid &e1, const Ellipsoid &e2) { assert_release(e1.size() == e2.size()); - //if(e1.is_unbounded() || e2.is_unbounded()) - // return Ellipsoid(e1.size()); - - //if(e1.is_empty() || e2.is_empty()) - // return Ellipsoid::empty(e1.size()); - auto Q1 = e1.G * e1.G.transpose(); auto Q2 = e2.G * e2.G.transpose(); @@ -176,14 +174,13 @@ namespace codac2 { assert(G.is_squared() && J.is_squared() && J_box.is_squared()); assert(n == J.cols() && n == J_box.cols() && n == q.size()); - Matrix JG = J * G; // note: reliability may be lost here! - IntervalMatrix G_(G); - IntervalMatrix JG_ = IntervalMatrix(JG); - IntervalVector unit_box(G.rows()); unit_box.init(Interval(-1,1)); + auto JG = J * G; // note: reliability may be lost here! + auto G_ = G.template cast(); + IntervalVector unit_box = IntervalVector::constant(G.rows(), {-1,1}); // normal case - IntervalMatrix I_ = IntervalMatrix(Eigen::MatrixXd::Identity(G.rows(),G.cols())); - IntervalMatrix JG_inv_(inverse_enclosure(JG)); // non rigourous inversion + IntervalMatrix I_ = IntervalMatrix::eye(G.rows(),G.cols()); + IntervalMatrix JG_inv_ = inverse_enclosure(JG); // rigourous inversion Matrix M(JG); auto W = JG_inv_; auto Z = I_; @@ -199,28 +196,26 @@ namespace codac2 { assert(q.size() == G.rows()); // SVD decomposition of JG = U*E*V.T - Eigen::BDCSVD bdcsvd(JG,Eigen::ComputeFullU); - IntervalMatrix U_(bdcsvd.matrixU()); // which is also the right part - Vector Sv(bdcsvd.singularValues()); // vectors of singular values + Eigen::BDCSVD bdcsvd(JG); + auto U_ = bdcsvd.matrixU().template cast(); // which is also the right part + auto Sv = bdcsvd.singularValues(); // vectors of singular values // select new singular values - int dim = G.rows(); - IntervalVector s_box(U_.transpose()*J_box*G_*unit_box); - IntervalMatrix S_(Eigen::MatrixXd::Zero(dim,dim)); // diagonal matrix of the new singular value - IntervalMatrix S_pinv_(Eigen::MatrixXd::Zero(dim,dim)); // pseudo inverse of S - for(int i=0;itrig[1]){ // normal size singular values S_(i,i) = Interval(Sv[i]); - S_pinv_(i,i) = 1/S_(i,i); }else{ // for very small singular values (0 included) use s_box - double val = s_box[i].ub(); - S_(i,i) = Interval(q[i]*val); - S_pinv_(i,i)=1/S_(i,i); + S_(i,i) = Interval(q[i])*s_box[i].ub(); } + S_pinv_(i,i) = 1./S_(i,i); } M = (U_*S_).mid(); W = S_pinv_*U_.transpose(); - Z = W*JG_; + Z = W*JG.template cast(); } auto b_box = (W * J_box * G_ - Z) * unit_box; @@ -253,7 +248,7 @@ namespace codac2 { ostream& operator<<(ostream& os, const Ellipsoid& e) { - os << "Ellipsoid:\n" + os << "Ellipsoid " << e.size() << "d:\n" << " mu=" << e.mu << "\n" << " G=\n" << e.G; return os; diff --git a/src/core/domains/ellipsoid/codac2_Ellipsoid.h b/src/core/domains/ellipsoid/codac2_Ellipsoid.h index 9202861b..a7ac9b3a 100644 --- a/src/core/domains/ellipsoid/codac2_Ellipsoid.h +++ b/src/core/domains/ellipsoid/codac2_Ellipsoid.h @@ -96,8 +96,9 @@ namespace codac2 * \param d a point on the plane * \param v a vector of the plane * \param u a other vector of the plane, orthogonal to v + * \return the projected 2d ellipsoid */ - void projection2D(const Vector& d, const Vector& v, const Vector& u); + Ellipsoid proj_2d(const Vector& d, const Vector& v, const Vector& u) const; public: diff --git a/src/core/domains/ellipsoid/codac2_Ellipsoid_utils.cpp b/src/core/domains/ellipsoid/codac2_Ellipsoid_utils.cpp index 2416c949..f14d3f2c 100644 --- a/src/core/domains/ellipsoid/codac2_Ellipsoid_utils.cpp +++ b/src/core/domains/ellipsoid/codac2_Ellipsoid_utils.cpp @@ -18,53 +18,66 @@ using codac2::BoolInterval; namespace codac2 { - Matrix solve_discrete_lyapunov(const Matrix& a,const Matrix& q) + Matrix solve_discrete_lyapunov(const Matrix& A, const Matrix& Q) { // implementation of the scipy solver for the discrete lyapunov equation (real matrix only) // works well under dimension 10 // https://github.com/scipy/scipy/blob/v1.14.1/scipy/linalg/_solvers.py#L235-L323 // Solves the discrete Lyapunov equation :math:`AXA^H - X + Q = 0` - assert(a.rows() == a.cols()); - assert(a.rows() == q.rows()); - assert(a.cols() == q.cols()); + assert(A.is_squared() && Q.is_squared()); + assert(A.size() == Q.size()); - Eigen::MatrixXd lhs = Eigen::KroneckerProduct(a, a); - lhs = Eigen::MatrixXd::Identity(lhs.rows(),lhs.cols()) - lhs; - Eigen::MatrixXd x = lhs.colPivHouseholderQr().solve((Eigen::VectorXd)q.reshaped()); - return Matrix(x.reshaped(q.rows(),q.cols())); + Matrix lhs = Eigen::KroneckerProduct(A,A); + lhs = Matrix::eye(lhs.rows(),lhs.cols()) - lhs; + Matrix x = lhs.colPivHouseholderQr().solve((Vector)Q.reshaped()); + return Matrix(x.reshaped(Q.rows(),Q.cols())); } - BoolInterval stability_analysis(const AnalyticFunction &f, int alpha_max, Ellipsoid &e, Ellipsoid &e_out) + BoolInterval stability_analysis(const AnalyticFunction &f, unsigned int alpha_max, Ellipsoid &e, Ellipsoid &e_out, bool verbose) { assert_release(f.args().size() == 1 && "f must have only one arg"); // get the Jacobian of f at the origin - int n = f.input_size(); + Index n = f.input_size(); Vector origin(Eigen::VectorXd::Zero(n)); Matrix J = f.diff(IntervalVector(origin)).mid(); // solve the axis aligned discrete lyapunov equation J.T * P * J − P = −J.T * J - Matrix P = solve_discrete_lyapunov(J.transpose(),J.transpose()*J); // TODO solve the Lyapunov equation !!! - Matrix G0((P.inverse()).sqrt()); - int alpha = 0; + auto P = solve_discrete_lyapunov(J.transpose(),J.transpose()*J); // TODO solve the Lyapunov equation !!! + auto G0 = P.inverse().sqrt(); + unsigned int alpha = 0; + + if(verbose) + cout << "Stability analysis:" << endl; while(alpha <= alpha_max) { - e = Ellipsoid(origin, std::pow(10,-alpha) * G0); + e = Ellipsoid(origin, std::pow(10.,-(int)alpha) * G0); e_out = nonlinear_mapping(e,f); - cout << "\nwith alpha = " << alpha << endl; - cout << "e is\n" << e << endl; - cout << "e_out is\n" << e_out << endl; + + if(verbose) + { + cout << "\t with alpha = " << alpha << endl; + cout << "\t e is\n" << e << endl; + cout << "\t e_out is\n" << e_out << endl; + } if(e_out.is_concentric_subset(e) == BoolInterval::TRUE) { - cout << "The system is stable" << endl; - cout << "Domain of attraction :\n" << e_out << endl; + if(verbose) + { + cout << "\t The system is stable" << endl; + cout << "\t Domain of attraction :\n" << e_out << endl; + } return BoolInterval::TRUE; } + alpha++; } - cout << "The method is not able to conclude on the stability" << endl; + + if(verbose) + cout << "\t The method is not able to conclude on the stability" << endl; + return BoolInterval::UNKNOWN; } } \ No newline at end of file diff --git a/src/core/domains/ellipsoid/codac2_Ellipsoid_utils.h b/src/core/domains/ellipsoid/codac2_Ellipsoid_utils.h index 48265836..45259423 100644 --- a/src/core/domains/ellipsoid/codac2_Ellipsoid_utils.h +++ b/src/core/domains/ellipsoid/codac2_Ellipsoid_utils.h @@ -20,16 +20,17 @@ namespace codac2 * \param alpha_max ... * \param e ... * \param e_out ... + * \param verbose ... * \return ... */ - BoolInterval stability_analysis(const AnalyticFunction& f, int alpha_max, Ellipsoid& e, Ellipsoid& e_out); + BoolInterval stability_analysis(const AnalyticFunction& f, unsigned int alpha_max, Ellipsoid& e, Ellipsoid& e_out, bool verbose = false); /** * \brief ... * - * \param a ... - * \param q ... + * \param A ... + * \param Q ... * \return ... */ - Matrix solve_discrete_lyapunov(const Matrix& a, const Matrix& q); + Matrix solve_discrete_lyapunov(const Matrix& A, const Matrix& Q); } \ No newline at end of file diff --git a/src/graphics/CMakeLists.txt b/src/graphics/CMakeLists.txt index ca7d530e..c18ba4b8 100644 --- a/src/graphics/CMakeLists.txt +++ b/src/graphics/CMakeLists.txt @@ -12,7 +12,6 @@ ${CMAKE_CURRENT_SOURCE_DIR}/3rd/vibes/vibes.h ${CMAKE_CURRENT_SOURCE_DIR}/figures/codac2_Figure2D.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/figures/codac2_Figure2D_ellipsoid.cpp ${CMAKE_CURRENT_SOURCE_DIR}/figures/codac2_Figure2D.h ${CMAKE_CURRENT_SOURCE_DIR}/figures/codac2_Figure2DInterface.h ${CMAKE_CURRENT_SOURCE_DIR}/figures/codac2_OutputFigure2D.cpp diff --git a/src/graphics/figures/codac2_Figure2D.cpp b/src/graphics/figures/codac2_Figure2D.cpp index b3af3ef9..9f9d210d 100644 --- a/src/graphics/figures/codac2_Figure2D.cpp +++ b/src/graphics/figures/codac2_Figure2D.cpp @@ -2,7 +2,7 @@ * codac2_Figure2D.cpp * ---------------------------------------------------------------------------- * \date 2024 - * \author Simon Rohou + * \author Simon Rohou, Morgan Louédec * \copyright Copyright 2024 Codac Team * \license GNU Lesser General Public License (LGPL) */ @@ -55,6 +55,16 @@ void Figure2D::set_axes(const FigureAxis& axis1, const FigureAxis& axis2) output_fig->update_axes(); } +const Index& Figure2D::i() const +{ + return axes()[0].dim_id; +} + +const Index& Figure2D::j() const +{ + return axes()[1].dim_id; +} + const Vector& Figure2D::pos() const { return _pos; @@ -199,6 +209,35 @@ void Figure2D::draw_ellipse(const Vector& c, const Vector& ab, double theta, con output_fig->draw_ellipse(c,ab,theta,s); } +void Figure2D::draw_ellipsoid(const Ellipsoid &e, const StyleProperties &s) { + // Author: Morgan Louédec + assert_release(this->size() <= e.size()); + + Index n = e.size(); + Ellipsoid proj_e(2); + + // 2d projection of the ellipsoid + if (n > 2) { + Vector v = Vector::zero(n); + v[i()] = 1; + Vector u = Vector::zero(n); + u[j()] = 1; + proj_e = e.proj_2d(Vector::zero(n), v, u); + } else { + proj_e = e; + } + + // draw the 2d ellipsoid + Eigen::JacobiSVD jsvd(proj_e.G, Eigen::ComputeThinU); + Matrix U(jsvd.matrixU()); + Vector ab(jsvd.singularValues()); + + double theta = std::atan2(U(1, 0), U(0, 0)); + + for(const auto& output_fig : _output_figures) + output_fig->draw_ellipse(proj_e.mu, ab, theta, s); +} + void Figure2D::draw_tank(const Vector& x, float size, const StyleProperties& s) { assert_release(this->size() <= x.size()+1); diff --git a/src/graphics/figures/codac2_Figure2D.h b/src/graphics/figures/codac2_Figure2D.h index 7ed01306..81e58764 100644 --- a/src/graphics/figures/codac2_Figure2D.h +++ b/src/graphics/figures/codac2_Figure2D.h @@ -70,6 +70,9 @@ namespace codac2 const std::vector& axes() const; void set_axes(const FigureAxis& axis1, const FigureAxis& axis2); + const Index& i() const; + const Index& j() const; + const Vector& pos() const; const Vector& window_size() const; void set_window_properties(const Vector& pos, const Vector& size); diff --git a/src/graphics/figures/codac2_Figure2D_ellipsoid.cpp b/src/graphics/figures/codac2_Figure2D_ellipsoid.cpp deleted file mode 100644 index 4d1c8373..00000000 --- a/src/graphics/figures/codac2_Figure2D_ellipsoid.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * codac2_Figure2D.cpp - * ---------------------------------------------------------------------------- - * \date 2024 - * \author Morgan Louédec - * \copyright Copyright 2024 Codac Team - * \license GNU Lesser General Public License (LGPL) - */ - -#include "codac2_Index.h" -#include "codac2_Figure2D.h" -#include "codac2_matrices.h" -#include "codac2_Matrix.h" - -using namespace std; -using codac2::Vector; -using codac2::Matrix; - -void codac2::Figure2D::draw_ellipsoid(const codac2::Ellipsoid &e, const codac2::StyleProperties &s) { - //assert_release(this->size() <= e.size()); - for (const auto &output_fig: _output_figures) { - Matrix G_draw(2, 2); - Vector mu_draw(2); - // 2d projection of the ellipsoid - if (e.size() > 2) { - // affine space of the projection - Vector d(Eigen::VectorXd::Zero(e.mu.rows())); - Matrix T(Eigen::MatrixXd::Zero(e.G.rows(), 2)); - T(output_fig->i(), 0) = 1; - T(output_fig->j(), 1) = 1; - - // project ellipsoid E(mu,Q) = {x in R^n | (x-mu).T*G.{-T}*G^{-1}*(x-mu)<1} - // on the affine plan A = {x|x=d+Tt} [Pope -2008] - // reduce the dimensions of mu and Q - - auto TTG = T.transpose().eval() * e.G; - Eigen::BDCSVD bdcsvd(Matrix(TTG.eval()), Eigen::ComputeFullU); - Matrix U(bdcsvd.matrixU()); - Matrix E((Eigen::MatrixXd) bdcsvd.singularValues().asDiagonal()); - G_draw = U * E; - mu_draw = T.transpose() * (d + T * T.transpose() * (e.mu - d)); - } else { - G_draw = e.G; - mu_draw = e.mu; - } - - // draw the 2d ellipsoid - Eigen::JacobiSVD jsvd(G_draw, Eigen::ComputeThinU); - Matrix U(jsvd.matrixU()); - Vector ab(jsvd.singularValues()); - - double theta = atan2(U(1, 0), U(0, 0)).mid(); - - output_fig->draw_ellipse(mu_draw, ab, theta, s); - } -} \ No newline at end of file diff --git a/src/graphics/figures/codac2_OutputFigure2D.cpp b/src/graphics/figures/codac2_OutputFigure2D.cpp index 56d37b26..32afc150 100644 --- a/src/graphics/figures/codac2_OutputFigure2D.cpp +++ b/src/graphics/figures/codac2_OutputFigure2D.cpp @@ -16,10 +16,10 @@ using namespace codac2; const Index& OutputFigure2D::i() const { - return _fig.axes()[0].dim_id; + return _fig.i(); } const Index& OutputFigure2D::j() const { - return _fig.axes()[1].dim_id; + return _fig.j(); } \ No newline at end of file