diff --git a/doc/manual/index.rst b/doc/manual/index.rst index d4c6b7c04..bbd315362 100644 --- a/doc/manual/index.rst +++ b/doc/manual/index.rst @@ -246,6 +246,7 @@ User manual * Geometric contractors * :ref:`sec-ctc-geom-ctcdist` * :ref:`sec-ctc-geom-ctcpolar` + * :ref:`sec-ctc-geom-ctcvisible` * CtcSegment * CtcPolygon * CtcPointCloud @@ -284,6 +285,7 @@ User manual * SepInverse * SepTransform * Geometrical separators + * SepVisible * SepPolarCart or SepCartPolar * SepPolygon * SepEllipse diff --git a/doc/manual/manual/contractors/geometric/ctcnovisible.png b/doc/manual/manual/contractors/geometric/ctcnovisible.png new file mode 100644 index 000000000..16d5457d1 Binary files /dev/null and b/doc/manual/manual/contractors/geometric/ctcnovisible.png differ diff --git a/doc/manual/manual/contractors/geometric/ctcvisible.png b/doc/manual/manual/contractors/geometric/ctcvisible.png new file mode 100644 index 000000000..99a4595ef Binary files /dev/null and b/doc/manual/manual/contractors/geometric/ctcvisible.png differ diff --git a/doc/manual/manual/contractors/geometric/ctcvisible.rst b/doc/manual/manual/contractors/geometric/ctcvisible.rst new file mode 100644 index 000000000..f75694d4e --- /dev/null +++ b/doc/manual/manual/contractors/geometric/ctcvisible.rst @@ -0,0 +1,113 @@ +.. _sec-ctc-geom-ctcvisible: + +The CtcVisible and CtcNoVisible contractors +=========================================== + + Main authors: `Quentin Brateau `_ + +The visibility constraint characterizes the set of points :math:`\mathbf{x} \in \mathbb{R}^2` that are visible from an observation point :math:`\mathbf{a}` given an obstacle segment :math:`[\mathbf{e}_1, \mathbf{e}_2]`. + +This constraint is based on the work of **Rémy Guyonneau** (see [Guyonneau2013]_). A point :math:`\mathbf{x}` is considered visible if the segment :math:`[\mathbf{a}, \mathbf{x}]` does not intersect the obstacle segment :math:`[\mathbf{e}_1, \mathbf{e}_2]`. This creates a "shadow cone" originating from :math:`\mathbf{a}`. + +.. image:: visibility.png + :alt: Illustration of the visibility constraint from an observation point [Guyonneau2013]_ + :width: 250px + +.. image:: novisibility.png + :alt: Illustration of the non-visibility constraint from an observation point [Guyonneau2013]_ + :width: 250px + +.. doxygenclass:: codac2::CtcVisible + :project: codac + +.. doxygenclass:: codac2::CtcNoVisible + :project: codac + +.. note:: + Current implementations assume a thin (degenerated) observation point :math:`\mathbf{a}`. Future versions will support visibility from **observation lines**, set defined by **convex polygons**, and from any **set** defined by a Separator. + +Methods +------- + +.. doxygenfunction:: codac2::CtcVisible::contract(IntervalVector&) const + :project: codac + +.. doxygenfunction:: codac2::CtcNoVisible::contract(IntervalVector&) const + :project: codac + + + +Example of use: Characterizing the set of visible and non-visible points from an observation point +-------------------------------------------------------------------------------------------------- + +In this example, we characterize the visible and hidden areas from an observer at the origin :math:`\mathbf{a}=(0,0)^\intercal` facing a wall represented by a segment from :math:`(2, -1)` to :math:`(2, 1)`. + +.. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src.py + :language: py + :start-after: [ctcvisible-beg] + :end-before: [ctcvisible-end] + :dedent: 4 + + .. group-tab:: C++ + + .. literalinclude:: src.cpp + :language: c++ + :start-after: [ctcvisible-beg] + :end-before: [ctcvisible-end] + :dedent: 4 + +.. figure:: ctcvisible.png + :width: 400px + +.. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src.py + :language: py + :start-after: [ctcnovisible-beg] + :end-before: [ctcnovisible-end] + :dedent: 4 + + .. group-tab:: C++ + + .. literalinclude:: src.cpp + :language: c++ + :start-after: [ctcnovisible-beg] + :end-before: [ctcnovisible-end] + :dedent: 4 + + Characterization of the visibility area. The points that are not visible are out of the visibility region and belongs to the blue area. The uncertain region is represented in yellow. + +.. figure:: ctcnovisible.png + :width: 400px + + Characterization of the non-visibility area. The points that are visible are out of the non-visibility region and belongs to the blue area. The uncertain region is represented in yellow. + +Fake Boundaries +--------------- + +When implementing visibility over a union of segments (e.g., a non-convex polygon), one must be careful of **fake boundaries**. These occur when the intersection of several visibility contractors creates "uncertain" regions that do not correspond to actual physical visibility limits. For a detailed discussion on handling these in interval analysis, see [Brateau2025]_. + +Related content +--------------- + +.. |visibility-pdf| replace:: **Download the manuscript** +.. _visibility-pdf: https://theses.hal.science/tel-00961501/file/these_remy_guyonneau.pdf + +.. |fake_boundaries-pdf| replace:: **Download the paper** +.. _fake_boundaries-pdf: https://cyber.bibl.u-szeged.hu/index.php/actcybern/article/view/4560 + +.. admonition:: References + + .. [Guyonneau2013] \ R. Guyonneau. *Modélisation et exploitation d'incertitudes pour la robotique mobile à l'aide de l'analyse par intervalles*. PhD Thesis, 2013. |visibility-pdf|_ + + .. [Brateau2025] \ Q. Brateau, et al. *Considering Adjacent Sets for Computing the Visibility Region*. Acta Cybernetica, 2025. |fake_boundaries-pdf|_ + +.. admonition:: Technical documentation + + See the `C++ API documentation of CtcVisible <../../api/html/classcodac2_1_1_ctc_visible.html>`_. \ No newline at end of file diff --git a/doc/manual/manual/contractors/geometric/index.rst b/doc/manual/manual/contractors/geometric/index.rst index d7e815fd8..938c40df6 100644 --- a/doc/manual/manual/contractors/geometric/index.rst +++ b/doc/manual/manual/contractors/geometric/index.rst @@ -5,6 +5,7 @@ Geometric contractors ctcdist.rst ctcpolar.rst + ctcvisible.rst CtcSegment CtcPolygon CtcEllipse diff --git a/doc/manual/manual/contractors/geometric/novisibility.png b/doc/manual/manual/contractors/geometric/novisibility.png new file mode 100644 index 000000000..669ef5b75 Binary files /dev/null and b/doc/manual/manual/contractors/geometric/novisibility.png differ diff --git a/doc/manual/manual/contractors/geometric/src.cpp b/doc/manual/manual/contractors/geometric/src.cpp index ce07ceb43..d2f6c942e 100644 --- a/doc/manual/manual/contractors/geometric/src.cpp +++ b/doc/manual/manual/contractors/geometric/src.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -86,4 +87,33 @@ TEST_CASE("CtcPolar - manual") // x = [1.5, 2.5] ; y = [6.53834, 7.85812] ; rho = [7, 8] ; theta = [1.20558, 1.38218] // [ctcpolar-2-end] } +} + +TEST_CASE("CtcVisible - manual") +{ + { + // [ctcvisible-beg] + Vector a({1, 1}); + Segment s({{1, 1}, {4, 4}}, {{3, 3}, {2, 2}}); + CtcVisible ctc(Vector({0.0, 0.0}), Segment({2.0, -1.0}, {2.0, 1.0})); + DefaultFigure::pave( + {{-1,6},{-1,6}}, + CtcNoVisible(a, s), + 1e-1 + ); + // [ctcvisible-end] + } + + { + // [ctcnovisible-beg] + Vector a({1, 1}); + Segment s({{1, 1}, {4, 4}}, {{3, 3}, {2, 2}}); + CtcNoVisible sep(a, s); + DefaultFigure::pave( + {{-1,6},{-1,6}}, + CtcNoVisible(a, s), + 1e-1 + ); + // [ctcnovisible-end] + } } \ No newline at end of file diff --git a/doc/manual/manual/contractors/geometric/src.py b/doc/manual/manual/contractors/geometric/src.py index c99ba1bb1..28ba25216 100644 --- a/doc/manual/manual/contractors/geometric/src.py +++ b/doc/manual/manual/contractors/geometric/src.py @@ -88,5 +88,29 @@ def tests_CtcPolar_manual(test): test.assertTrue(Approx(rho,1e-5) == Interval([7,8])) test.assertTrue(Approx(theta,1e-5) == Interval([1.20558,1.38218])) + def test_CtcVisible(test): + # [ctcvisible-beg] + a = [1, 1] + s = Segment([1, 4], [3, 2]) + ctc = CtcVisible(a, s) + DefaultFigure.pave( + [[-1,6],[-1,6]], + ctc, + 0.1 + ) + # [ctcvisible-end] + + # [ctcnovisible-beg] + a = [1, 1] + s = Segment([1, 4], [3, 2]) + ctc = CtcNoVisible(a, s) + DefaultFigure.pave( + [[-1,6],[-1,6]], + ctc, + 0.1 + ) + # [ctcnovisible-end] + + if __name__ == '__main__': unittest.main() \ No newline at end of file diff --git a/doc/manual/manual/contractors/geometric/visibility.png b/doc/manual/manual/contractors/geometric/visibility.png new file mode 100644 index 000000000..b1f016dbc Binary files /dev/null and b/doc/manual/manual/contractors/geometric/visibility.png differ diff --git a/doc/manual/manual/contractors/index.rst b/doc/manual/manual/contractors/index.rst index e86d51bdc..41217f82d 100644 --- a/doc/manual/manual/contractors/index.rst +++ b/doc/manual/manual/contractors/index.rst @@ -7,6 +7,7 @@ Contractors, separators CtcInverse CtcDist CtcPolar + CtcVisible .. What are contractors? .. The Ctc class diff --git a/examples/14_visibility/CMakeLists.txt b/examples/14_visibility/CMakeLists.txt new file mode 100644 index 000000000..cf9ef35ce --- /dev/null +++ b/examples/14_visibility/CMakeLists.txt @@ -0,0 +1,34 @@ +# ================================================================== +# codac / basics example - cmake configuration file +# ================================================================== + + cmake_minimum_required(VERSION 3.5) + project(codac_example LANGUAGES CXX) + + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Adding Codac + + # In case you installed Codac in a local directory, you need + # to specify its path with the CMAKE_PREFIX_PATH option. + # set(CMAKE_PREFIX_PATH "~/codac/build_install") + + find_package(CODAC REQUIRED) + message(STATUS "Found Codac version ${CODAC_VERSION}") + +# Initializating Ibex + + ibex_init_common() + +# Compilation + + if(FAST_RELEASE) + add_compile_definitions(FAST_RELEASE) + message(STATUS "You are running Codac in fast release mode. (option -DCMAKE_BUILD_TYPE=Release is required)") + endif() + + add_executable(${PROJECT_NAME} main.cpp) + target_compile_options(${PROJECT_NAME} PUBLIC ${CODAC_CXX_FLAGS}) + target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${CODAC_INCLUDE_DIRS}) + target_link_libraries(${PROJECT_NAME} PUBLIC ${CODAC_LIBRARIES}) \ No newline at end of file diff --git a/examples/14_visibility/main.cpp b/examples/14_visibility/main.cpp new file mode 100644 index 000000000..9c4edd85c --- /dev/null +++ b/examples/14_visibility/main.cpp @@ -0,0 +1,23 @@ +#include +using namespace codac2; + +int main() +{ + // Observation Point and Obstacle Segment + Vector a({1, 1}); + Segment s({{1,1}, {4,4}}, {{3,3}, {2,2}}); + + // Set up the figure + DefaultFigure::set_axes(axis(0,{-1,6}), axis(1,{-1,6})); + + // Show the observation point and the segment + DefaultFigure::draw_circle(a, 0.05, StyleProperties({Color::dark_green(), Color::green()}, "w:0.025", "z:5")); + DefaultFigure::draw_line(s[0].mid(), s[1].mid(), StyleProperties(Color::red(), "w:0.05", "z:5")); + + // Paving of the visibility separator + DefaultFigure::pave( + {{-1,6},{-1,6}}, + CtcVisible(a, s), + 1e-1 + ); +} \ No newline at end of file diff --git a/python/src/core/CMakeLists.txt b/python/src/core/CMakeLists.txt index 394d6a589..32f00d214 100644 --- a/python/src/core/CMakeLists.txt +++ b/python/src/core/CMakeLists.txt @@ -36,7 +36,8 @@ contractors/codac2_py_CtcQInter.cpp contractors/codac2_py_CtcSegment.cpp contractors/codac2_py_CtcUnion.cpp - contractors/codac2_py_CtcWrapper.cpp + contractors/codac2_py_CtcUnion.cpp + contractors/codac2_py_CtcVisible.cpp contractors/codac2_py_linear_ctc.cpp domains/ellipsoid/codac2_py_Ellipsoid.cpp @@ -113,6 +114,7 @@ separators/codac2_py_SepQInter.cpp separators/codac2_py_SepTransform.cpp separators/codac2_py_SepUnion.cpp + separators/codac2_py_SepVisible.cpp separators/codac2_py_SepWrapper.cpp tools/codac2_py_Approx.cpp diff --git a/python/src/core/contractors/codac2_py_CtcVisible.cpp b/python/src/core/contractors/codac2_py_CtcVisible.cpp new file mode 100644 index 000000000..df1591585 --- /dev/null +++ b/python/src/core/contractors/codac2_py_CtcVisible.cpp @@ -0,0 +1,42 @@ +/** * Codac binding (core) + * ---------------------------------------------------------------------------- + * \date 2026 + * \author Quentin Brateau + * \copyright Copyright 2026 Codac Team + * \license GNU Lesser General Public License (LGPL) + */ +#include +#include +#include +#include "codac2_py_Ctc.h" +#include "codac2_py_CtcWrapper_docs.h" // Generated file from Doxygen XML (doxygen2docstring.py): + +#define CTCVISIBLE_MAIN "Contractor for visibility from a point relative to a segment." +#define CTCVISIBLE_INIT "Initialize CtcVisible with an observation point and an obstacle segment." +#define CTCVISIBLE_CONTRACT "Contract the box x based on visibility criteria." + +#define CTCNOVISIBLE_MAIN "Contractor for the hidden zone (shadow) behind a segment." +#define CTCNOVISIBLE_INIT "Initialize CtcNoVisible with an observation point and an obstacle segment." +#define CTCNOVISIBLE_CONTRACT "Contract the box x to the hidden area." + +using namespace std; +using namespace codac2; +namespace py = pybind11; +using namespace pybind11::literals; + +void export_CtcVisibility(py::module& m, py::class_, pyCtcIntervalVector>& pyctc) +{ + // --- CtcVisible --- + py::class_ vis(m, "CtcVisible", pyctc, CTCVISIBLE_MAIN); + vis + .def(py::init(), + CTCVISIBLE_INIT, "a"_a, "s"_a) + .def(CONTRACT_BOX_METHOD(CtcVisible, CTCVISIBLE_CONTRACT)); + + // --- CtcNoVisible --- + py::class_ nvis(m, "CtcNoVisible", pyctc, CTCNOVISIBLE_MAIN); + nvis + .def(py::init(), + CTCNOVISIBLE_INIT, "a"_a, "s"_a) + .def(CONTRACT_BOX_METHOD(CtcNoVisible, CTCNOVISIBLE_CONTRACT)); +} \ No newline at end of file diff --git a/python/src/core/separators/codac2_py_SepVisible.cpp b/python/src/core/separators/codac2_py_SepVisible.cpp new file mode 100644 index 000000000..9b3b76ff0 --- /dev/null +++ b/python/src/core/separators/codac2_py_SepVisible.cpp @@ -0,0 +1,36 @@ +/** * Codac binding (core) + * ---------------------------------------------------------------------------- + * \date 2026 + * \author Quentin Brateau + * \copyright Copyright 2026 Codac Team + * \license GNU Lesser General Public License (LGPL) + */ + +#include +#include +#include +#include "codac2_py_Sep.h" +#include "codac2_py_SepUnion_docs.h" // Generated file from Doxygen XML (doxygen2docstring.py): + +#define SEPVISIBILITY_MAIN "Separator for visibility characterization (Inside=Hidden, Outside=Visible)." +#define SEPVISIBILITY_INIT "Initialize SepVisible with an observation point 'a' and an obstacle segment 's'." +#define SEPVISIBILITY_SEPARATE "Separate the box into hidden and visible parts, returning a tuple (x_in, x_out)." + +using namespace std; +using namespace codac2; +namespace py = pybind11; +using namespace pybind11::literals; + +void export_SepVisible(py::module& m, py::class_& pysep) +{ + py::class_ exported(m, "SepVisible", pysep, SEPVISIBILITY_MAIN); + exported + .def(py::init(), + "a"_a, "s"_a, + SEPVISIBILITY_INIT) + + .def("separate", &SepVisible::separate, + "x"_a, + SEPVISIBILITY_SEPARATE) + ; +} \ No newline at end of file diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 51ccb14dd..129ac2704 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -39,6 +39,8 @@ ${CMAKE_CURRENT_SOURCE_DIR}/contractors/codac2_CtcLazy.h ${CMAKE_CURRENT_SOURCE_DIR}/contractors/codac2_CtcNot.h ${CMAKE_CURRENT_SOURCE_DIR}/contractors/codac2_CtcUnion.h + ${CMAKE_CURRENT_SOURCE_DIR}/contractors/codac2_CtcVisible.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/contractors/codac2_CtcVisible.h ${CMAKE_CURRENT_SOURCE_DIR}/contractors/codac2_CtcPointCloud.cpp ${CMAKE_CURRENT_SOURCE_DIR}/contractors/codac2_CtcPointCloud.h ${CMAKE_CURRENT_SOURCE_DIR}/contractors/codac2_CtcPolar.cpp @@ -244,6 +246,7 @@ ${CMAKE_CURRENT_SOURCE_DIR}/separators/codac2_SepTransform.h ${CMAKE_CURRENT_SOURCE_DIR}/separators/codac2_SepUnion.cpp ${CMAKE_CURRENT_SOURCE_DIR}/separators/codac2_SepUnion.h + ${CMAKE_CURRENT_SOURCE_DIR}/separators/codac2_SepVisible.h ${CMAKE_CURRENT_SOURCE_DIR}/separators/codac2_SepWrapper.cpp ${CMAKE_CURRENT_SOURCE_DIR}/separators/codac2_SepWrapper.h diff --git a/src/core/contractors/codac2_CtcVisible.cpp b/src/core/contractors/codac2_CtcVisible.cpp new file mode 100644 index 000000000..721edd6fe --- /dev/null +++ b/src/core/contractors/codac2_CtcVisible.cpp @@ -0,0 +1,172 @@ +/** * codac2_CtcVisible.cpp + * ---------------------------------------------------------------------------- + * \date 2026 + * \author Quentin Brateau + * \copyright Copyright 2026 Codac Team + * \license GNU Lesser General Public License (LGPL) + */ + + + /** * codac2_CtcVisible.cpp + */ + +#include "codac2_CtcVisible.h" +#include "codac2_det.h" +#include "codac2_arith_sub.h" +#include "codac2_max.h" +#include "codac2_min.h" +#include "codac2_IntervalVector.h" + + +using namespace std; +using namespace codac2; + +CtcVisible::CtcVisible(const Vector& a, const Segment& s) + : Ctc(2), + _a(a), _s(s), + _v_e2e1(s[1] - s[0]), + _v_ae1(a - s[0]), + _v_ae2(a - s[1]) +{ + // Compute orientation (ksi) + double det_val = (_a[0].mid() - _s[0][0].mid()) * (_v_e2e1[1].mid()) - + (_a[1].mid() - _s[0][1].mid()) * (_v_e2e1[0].mid()); + _k = (det_val > 0) ? 1.0 : -1.0; +} + +void CtcVisible::contract(IntervalVector& x) const +{ + IntervalVector x1(x), x2(x), x3(x), x4(x); + + contract_det(x1, _s[0], _v_e2e1, _k); + contract_det(x2, _s[0], _v_ae1, _k); + contract_det(x3, _s[1], _v_ae2, -_k); + contract_aabb(x4); + + x &= (x1 | x2 | x3 | x4); +} + +void CtcVisible::contract_det(IntervalVector& x, const IntervalVector& p, const IntervalVector& v, double sign) const +{ + IntervalVector v_xp = x - p; + IntervalVector v_fixed = v; + + Interval target = (sign > 0) ? Interval(0, oo) : Interval(-oo, 0); + + DetOp::bwd(target, v_xp, v_fixed); + x &= v_xp + p; +} + +void CtcVisible::contract_aabb(IntervalVector& x) const +{ + auto contract_1dim = [](double a, Interval& x_val, double c, double d) { + double min_cd = std::min(c, d); + double max_cd = std::max(c, d); + + // Forward contractions + Interval i1 = MinOp::fwd(Interval(a), x_val); + Interval i2 = MaxOp::fwd(i1, Interval(min_cd)); + Interval i3 = MaxOp::fwd(Interval(a), x_val); + Interval i4 = MinOp::fwd(i3, Interval(max_cd)); + Interval i5 = i2 - i4; + + // Top of the DAG + if ((i5 &= Interval(0, oo)).is_empty()) { + x_val.set_empty(); + return; + } + + // Backward contractions + i2 &= i5 + i4; + i4 &= i2 - i5; + + Interval tmp_max_cd = Interval(max_cd); + Interval tmp_min_cd = Interval(min_cd); + Interval tmp_a = Interval(a); + + MinOp::bwd(i4, i3, tmp_max_cd); + + tmp_a = Interval(a); // reset + MaxOp::bwd(i3, tmp_a, x_val); + + MaxOp::bwd(i2, i1, tmp_min_cd); + + tmp_a = Interval(a); // reset + MinOp::bwd(i1, tmp_a, x_val); + }; + + // Apply the 1D contraction to each dimension + // Note: _a and _s are assumed to be Point-like (degenerate intervals) + // so we use .mid() to get the double values c, d, and a. + contract_1dim(_a[0].mid(), x[0], _s[0][0].mid(), _s[1][0].mid()); + contract_1dim(_a[1].mid(), x[1], _s[0][1].mid(), _s[1][1].mid()); +} + +CtcNoVisible::CtcNoVisible(const IntervalVector& a, const Segment& s) + : Ctc(2), + _a(a), _s(s), + _v_e2e1(s[1] - s[0]), + _v_ae1(a - s[0]), + _v_ae2(a - s[1]) +{ + double det_val = (_a[0].mid() - _s[0][0].mid()) * (_v_e2e1[1].mid()) - + (_a[1].mid() - _s[0][1].mid()) * (_v_e2e1[0].mid()); + _k = (det_val > 0) ? 1.0 : -1.0; +} + +void CtcNoVisible::contract(IntervalVector& x) const +{ + IntervalVector xi(x); + contract_det(xi, _s[0], _v_e2e1, -_k); + contract_det(xi, _s[0], _v_ae1, -_k); + contract_det(xi, _s[1], _v_ae2, _k); + contract_aabb(xi); + + x &= xi; +} + +void CtcNoVisible::contract_det(IntervalVector& x, const IntervalVector& p, const IntervalVector& v, double sign) const +{ + IntervalVector v_xp = x - p; + IntervalVector v_fixed = v; + Interval target = (sign > 0) ? Interval(0, oo) : Interval(-oo, 0); + + DetOp::bwd(target, v_xp, v_fixed); + x &= v_xp + p; +} + +void CtcNoVisible::contract_aabb(IntervalVector& x) const +{ + auto contract_1dim = [](double a, Interval& x_val, double c, double d) { + double min_cd = std::min(c, d); + double max_cd = std::max(c, d); + + Interval i1 = MinOp::fwd(Interval(a), x_val); + Interval i2 = MaxOp::fwd(i1, Interval(min_cd)); + Interval i3 = MaxOp::fwd(Interval(a), x_val); + Interval i4 = MinOp::fwd(i3, Interval(max_cd)); + Interval i5 = i2 - i4; + + if ((i5 &= Interval(-oo, 0)).is_empty()) { + x_val.set_empty(); + return; + } + + i2 &= i5 + i4; + i4 &= i2 - i5; + + Interval tmp_max_cd = Interval(max_cd); + Interval tmp_min_cd = Interval(min_cd); + Interval tmp_a = Interval(a); + + MinOp::bwd(i4, i3, tmp_max_cd); + tmp_a = Interval(a); + MaxOp::bwd(i3, tmp_a, x_val); + MaxOp::bwd(i2, i1, tmp_min_cd); + tmp_a = Interval(a); + MinOp::bwd(i1, tmp_a, x_val); + }; + + contract_1dim(_a[0].mid(), x[0], _s[0][0].mid(), _s[1][0].mid()); + contract_1dim(_a[1].mid(), x[1], _s[0][1].mid(), _s[1][1].mid()); +} diff --git a/src/core/contractors/codac2_CtcVisible.h b/src/core/contractors/codac2_CtcVisible.h new file mode 100644 index 000000000..89884d86f --- /dev/null +++ b/src/core/contractors/codac2_CtcVisible.h @@ -0,0 +1,60 @@ +/** * \file codac2_CtcVisible.h + * ---------------------------------------------------------------------------- + * \date 2026 + * \author Quentin Brateau + * \copyright Copyright 2026 Codac Team + * \license GNU Lesser General Public License (LGPL) + */ + +#pragma once + +#include "codac2_Ctc.h" +#include "codac2_Segment.h" + +namespace codac2 +{ + class CtcVisible : public Ctc + { + public: + /** + * \brief Constructor for visibility from point 'a' relative to segment 's'. + */ + CtcVisible(const Vector& a, const Segment& s); + + void contract(IntervalVector& x) const; + + private: + const IntervalVector _a; + const Segment _s; + + // Pre-calculated constants for the obstacle + const IntervalVector _e1, _e2; + const IntervalVector _v_e2e1; // e2 - e1 + const IntervalVector _v_ae1; // a - e1 + const IntervalVector _v_ae2; // a - e2 + const IntervalVector _s_box; // Bounding box of the segment + + double _k; // Orientation sign (ksi) + + // Internal helpers for the 4 conditions + void contract_det(IntervalVector& x, const IntervalVector& p, const IntervalVector& v, double sign) const; + void contract_aabb(IntervalVector& x) const; + }; + + class CtcNoVisible : public Ctc + { + public: + CtcNoVisible(const IntervalVector& a, const Segment& s); + + void contract(IntervalVector& x) const; + + private: + const IntervalVector _a; + const Segment _s; + const IntervalVector _v_e2e1, _v_ae1, _v_ae2; + double _k; + + void contract_det(IntervalVector& x, const IntervalVector& p, const IntervalVector& v, double sign) const; + void contract_aabb(IntervalVector& x) const; + }; +} \ No newline at end of file diff --git a/src/core/separators/codac2_SepVisible.h b/src/core/separators/codac2_SepVisible.h new file mode 100644 index 000000000..081c4f332 --- /dev/null +++ b/src/core/separators/codac2_SepVisible.h @@ -0,0 +1,41 @@ +/** + * \file codac2_SepVisible.h + * ---------------------------------------------------------------------------- + * \date 2026 + * \author Quentin Brateau + * \copyright Copyright 2026 Codac Team + * \license GNU Lesser General Public License (LGPL) + */ + +#pragma once + +#include "codac2_Sep.h" +#include "codac2_CtcVisible.h" + +namespace codac2 { + class SepVisible : public Sep { + public: + + SepVisible(const Vector& a, const Segment& s) + : Sep(2), + _ctc_visible(a, s), + _ctc_novisible(a, s) + {} + + BoxPair separate(const IntervalVector& x) const override { + IntervalVector x_in(x); + IntervalVector x_out(x); + + _ctc_novisible.contract(x_in); + _ctc_visible.contract(x_out); + + assert((x_in | x_out) == x); + + return {x_in, x_out}; + } + + private: + const CtcVisible _ctc_visible; + const CtcNoVisible _ctc_novisible; + }; +} \ No newline at end of file diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b3eded7af..7507b2cf7 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -40,6 +40,7 @@ list(APPEND SRC_TESTS # listing files without extension core/contractors/codac2_tests_CtcLazy core/contractors/codac2_tests_CtcPolygon core/contractors/codac2_tests_CtcSegment + core/contractors/codac2_tests_CtcVisible core/contractors/codac2_tests_linear_ctc ../doc/manual/manual/contractors/geometric/src ../doc/manual/manual/contractors/analytic/src @@ -91,6 +92,7 @@ list(APPEND SRC_TESTS # listing files without extension core/separators/codac2_tests_SepPolygon core/separators/codac2_tests_SepProj core/separators/codac2_tests_SepTransform + core/separators/codac2_tests_SepVisible core/tools/codac2_tests_Approx core/tools/codac2_tests_serialization diff --git a/tests/core/contractors/codac2_tests_CtcVisible.cpp b/tests/core/contractors/codac2_tests_CtcVisible.cpp new file mode 100644 index 000000000..9b5e96ed3 --- /dev/null +++ b/tests/core/contractors/codac2_tests_CtcVisible.cpp @@ -0,0 +1,102 @@ +/** * Codac tests - Visibility Contractors + * ---------------------------------------------------------------------------- + * \date 2026 + * \author Quentin Brateau + * \copyright Copyright 2026 Codac Team + * \license GNU Lesser General Public License (LGPL) + */ + +#include +#include +#include + +using namespace codac2; + +TEST_CASE("CtcVisible & CtcNoVisible - Point/Segment visibility") +{ + // Observer at origin, obstacle horizontal from (2, -1) to (2, 1) + Vector a({0.0, 0.0}); + Segment s({2.0, -1.0}, {2.0, 1.0}); + + CtcVisible ctc_vis(a, s); + CtcNoVisible ctc_no_vis(a, s); + + SECTION("Box fully in visible zone (in front of obstacle)") + { + IntervalVector x({{0.5, 1.5}, {-0.5, 0.5}}); + IntervalVector x_orig = x; + + ctc_vis.contract(x); + CHECK(x == x_orig); // Should not be contracted + + ctc_no_vis.contract(x_orig); + CHECK(x_orig.is_empty()); // Cannot be hidden if in front + } + + SECTION("Box fully in hidden zone (shadow)") + { + IntervalVector x({{3.0, 4.0}, {-0.2, 0.2}}); + IntervalVector x_orig = x; + + ctc_vis.contract(x); + CHECK(x.is_empty()); // Fully in shadow -> not visible + + ctc_no_vis.contract(x_orig); + CHECK(x_orig == IntervalVector({{3.0, 4.0}, {-0.2, 0.2}})); // Fully hidden + } + + SECTION("Box behind the observer") + { + IntervalVector x({{-2.0, -1.0}, {-1.0, 1.0}}); + IntervalVector x_orig = x; + + ctc_vis.contract(x); + CHECK(x == x_orig); // Visible (obstacle is far away in the other direction) + + ctc_no_vis.contract(x_orig); + CHECK(x_orig.is_empty()); + } + + SECTION("Box on the side (outside the angular cone)") + { + IntervalVector x({{1.0, 4.0}, {2.0, 3.0}}); // High above the obstacle + IntervalVector x_orig = x; + + ctc_vis.contract(x); + CHECK(x == x_orig); + + ctc_no_vis.contract(x_orig); + CHECK(x_orig.is_empty()); + } + + SECTION("Straddling the shadow edge (angular boundary)") + { + // Obstacle is y in [-1, 1] at x=2. Upper shadow boundary is line y = 0.5*x (roughly) + // We place a box at x=4, y in [1.5, 2.5]. + // Boundary at x=4 is y=2. So [1.5, 2] is hidden, [2, 2.5] is visible. + + IntervalVector x_vis({{4.0, 4.0}, {1.5, 2.5}}); + ctc_vis.contract(x_vis); + // Note: Depends on precision/ksi, but x_vis[1] should be pruned to [2, 2.5] + CHECK(x_vis[1].lb() >= 1.99); + + IntervalVector x_hid({{4.0, 4.0}, {1.5, 2.5}}); + ctc_no_vis.contract(x_hid); + CHECK(x_hid[1].ub() <= 2.01); + } + + SECTION("AABB boundary test (Cinterseg logic)") + { + // Box is angularly behind the segment, but outside its X-AABB + // Observer (0,0), Segment x in [2,3], y=0. + // Box at x=1.5 (between observer and obstacle) + Segment s2({2.0, 0.0}, {3.0, 0.0}); + Vector a2({0.0, 0.0}); + CtcVisible c_vis2(a2, s2); + + IntervalVector x({{1.0, 1.5}, {-0.5, 0.5}}); + IntervalVector x_orig = x; + c_vis2.contract(x); + CHECK(x == x_orig); // Visible because it's in front of the obstacle's X-range + } +} \ No newline at end of file diff --git a/tests/core/contractors/codac2_tests_CtcVisible.py b/tests/core/contractors/codac2_tests_CtcVisible.py new file mode 100644 index 000000000..4b03c22de --- /dev/null +++ b/tests/core/contractors/codac2_tests_CtcVisible.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python + +# Codac tests - Visibility +# ---------------------------------------------------------------------------- +# \date 2026 +# \author Quentin Brateau +# \copyright Copyright 2026 Codac Team +# \license GNU Lesser General Public License (LGPL) + +import unittest +from codac import * + +class TestVisibility(unittest.TestCase): + + def test_CtcVisibility(self): + # Observer at origin, obstacle vertical at x=2 from y=-1 to y=1 + a = [0.0, 0.0] + s = Segment([2.0, -1.0], [2.0, 1.0]) + + ctc_vis = CtcVisible(a, s) + ctc_nvis = CtcNoVisible(a, s) + + # 1. Box fully in visible zone (in front of obstacle) + x = IntervalVector([[0.5, 1.5], [-0.5, 0.5]]) + x_orig = IntervalVector(x) + ctc_vis.contract(x) + self.assertTrue(x == x_orig) + + x_test_no = IntervalVector(x_orig) + ctc_nvis.contract(x_test_no) + self.assertTrue(x_test_no.is_empty()) + + # 2. Box fully in hidden zone (shadow) + x = IntervalVector([[3.0, 4.0], [-0.2, 0.2]]) + x_orig = IntervalVector(x) + ctc_vis.contract(x) + self.assertTrue(x.is_empty()) + + x_test_no = IntervalVector(x_orig) + ctc_nvis.contract(x_test_no) + self.assertTrue(x_test_no == x_orig) + + # 3. Box behind the observer + x = IntervalVector([[-2.0, -1.0], [-1.0, 1.0]]) + x_orig = IntervalVector(x) + ctc_vis.contract(x) + self.assertTrue(x == x_orig) + + x_test_no = IntervalVector(x_orig) + ctc_nvis.contract(x_test_no) + self.assertTrue(x_test_no.is_empty()) + + # 4. Box on the side (outside the angular cone) + x = IntervalVector([[1.0, 4.0], [2.0, 3.0]]) + x_orig = IntervalVector(x) + ctc_vis.contract(x) + self.assertTrue(x == x_orig) + + x_test_no = IntervalVector(x_orig) + ctc_nvis.contract(x_test_no) + self.assertTrue(x_test_no.is_empty()) + + # 5. Straddling the shadow edge (angular boundary) + # Boundary at x=4 is y=2. Visible: [2, 2.5], Hidden: [1.5, 2] + x_vis = IntervalVector([[4.0, 4.0], [1.5, 2.5]]) + ctc_vis.contract(x_vis) + self.assertGreaterEqual(x_vis[1].lb(), 1.99) + + x_hid = IntervalVector([[4.0, 4.0], [1.5, 2.5]]) + ctc_nvis.contract(x_hid) + self.assertLessEqual(x_hid[1].ub(), 2.01) + + # 6. AABB boundary test (Sight-line doesn't reach obstacle) + s2 = Segment([2.0, 0.0], [3.0, 0.0]) + ctc_vis2 = CtcVisible([0.0, 0.0], s2) + x = IntervalVector([[1.0, 1.5], [-0.5, 0.5]]) + x_orig = IntervalVector(x) + ctc_vis2.contract(x) + self.assertTrue(x == x_orig) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/tests/core/separators/codac2_tests_SepVisible.cpp b/tests/core/separators/codac2_tests_SepVisible.cpp new file mode 100644 index 000000000..20dd82c96 --- /dev/null +++ b/tests/core/separators/codac2_tests_SepVisible.cpp @@ -0,0 +1,52 @@ +/** * Codac tests - Visibility Separator + * ---------------------------------------------------------------------------- + * \date 2026 + * \author Quentin Brateau + * \copyright Copyright 2024 Codac Team + * \license GNU Lesser General Public License (LGPL) + */ + +#include +#include + +using namespace codac2; + +TEST_CASE("SepVisible - Space Partitioning") +{ + Vector a({0.0, 0.0}); + Segment s({1.0, 1.0}, {1.0, -1.0}); // Vertical wall at x=1 + SepVisible sep(a, s); + + SECTION("Consistency check: in | out should cover boundary") + { + IntervalVector x({{0.0, 2.0}, {-2.0, 2.0}}); + BoxPair res = sep.separate(x); + + // The union of the contracted boxes should ideally cover the original box + // (minus the parts definitively removed by contractors) + CHECK(!res.inner.is_empty()); // Some parts are hidden (x > 1) + CHECK(!res.outer.is_empty()); // Some parts are visible (x < 1 or y > cone) + } + + SECTION("Corner case: Box exactly on the observation point") + { + // A point at the source is always visible (or at least not hidden by the obstacle) + IntervalVector x({{0.0, 0.0}, {0.0, 0.0}}); + BoxPair res = sep.separate(x); + + CHECK(res.inner.is_empty()); // Not hidden + CHECK(res.outer == x); // Visible + } + + SECTION("Degenerate Obstacle (Segment of length 0)") + { + // If the segment is just a point, the shadow is just a ray (infinitely thin) + Segment s_null({1.0, 0.0}, {1.0, 0.0}); + SepVisible sep_null(a, s_null); + + IntervalVector x({{2.0, 3.0}, {-1.0, 1.0}}); + BoxPair res = sep_null.separate(x); + + CHECK(res.inner.is_empty()); + } +} \ No newline at end of file diff --git a/tests/core/separators/codac2_tests_SepVisible.py b/tests/core/separators/codac2_tests_SepVisible.py new file mode 100644 index 000000000..cdad5c61c --- /dev/null +++ b/tests/core/separators/codac2_tests_SepVisible.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python + +# Codac tests - Visibility +# ---------------------------------------------------------------------------- +# \date 2026 +# \author Quentin Brateau +# \copyright Copyright 2026 Codac Team +# \license GNU Lesser General Public License (LGPL) + +import unittest +from codac import * + +class TestVisibility(unittest.TestCase): + + def test_SepVisible(self): + a = [0.0, 0.0] + s = Segment([1.0, 1.0], [1.0, -1.0]) + sep = SepVisible(a, s) + + # 1. Space Partitioning + x = IntervalVector([[0.0, 2.0], [-2.0, 2.0]]) + x_in, x_out = sep.separate(x) + self.assertFalse(x_in.is_empty()) + self.assertFalse(x_out.is_empty()) + + # 2. Box exactly on observation point + x_point = IntervalVector([[0.0, 0.0], [0.0, 0.0]]) + x_in, x_out = sep.separate(x_point) + self.assertTrue(x_in.is_empty()) + self.assertTrue(x_out == x_point) + + # 3. Box entirely in shadow + x_shadow = IntervalVector([[2.0, 3.0], [-0.1, 0.1]]) + x_in, x_out = sep.separate(x_shadow) + self.assertTrue(x_out.is_empty()) + self.assertTrue(x_in == x_shadow) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file