├── .gitignore ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── tests ├── catch.hpp ├── main.cpp ├── primitives_test.cpp ├── vector2_test.cpp └── visibility_test.cpp └── visibility ├── floats.hpp ├── primitives.hpp ├── vector2.hpp └── visibility.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | build_gcc -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | compiler: 3 | - gcc 4 | before_install: 5 | - sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y 6 | - sudo apt-get update -qq 7 | - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-5; fi 8 | - if [ "$CXX" = "g++" ]; then export CXX="g++-5" CC="gcc-5"; fi 9 | before_script: 10 | - mkdir build 11 | - cd build 12 | - cmake .. 13 | script: 14 | - make 15 | - ./tests -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.0) 2 | project(visibility) 3 | 4 | set(visibility_headers 5 | ${PROJECT_SOURCE_DIR}/visibility/floats.hpp 6 | ${PROJECT_SOURCE_DIR}/visibility/vector2.hpp 7 | ${PROJECT_SOURCE_DIR}/visibility/primitives.hpp 8 | ${PROJECT_SOURCE_DIR}/visibility/visibility.hpp 9 | ) 10 | 11 | set(all_tests 12 | ${PROJECT_SOURCE_DIR}/tests/catch.hpp 13 | ${PROJECT_SOURCE_DIR}/tests/main.cpp 14 | ${PROJECT_SOURCE_DIR}/tests/vector2_test.cpp 15 | ${PROJECT_SOURCE_DIR}/tests/primitives_test.cpp 16 | ${PROJECT_SOURCE_DIR}/tests/visibility_test.cpp 17 | ) 18 | 19 | include_directories(${PROJECT_SOURCE_DIR}) 20 | 21 | if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") 22 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Werror -Wextra") 23 | endif() 24 | 25 | # Create project in MSVC 26 | add_library(visibility STATIC ${visibility_headers}) 27 | set_target_properties(visibility PROPERTIES LINKER_LANGUAGE CXX) 28 | 29 | add_executable(tests ${all_tests}) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 trylock 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Visibility Polygon 2 | [![Build Status](https://travis-ci.org/trylock/visibility.svg?branch=master)](https://travis-ci.org/trylock/visibility) 3 | 4 | Simple sweep line [Visibility polygon](https://en.wikipedia.org/wiki/Visibility_polygon) algorithm implementation. My inspiration for the project was [this 2d Visibility](http://www.redblobgames.com/articles/visibility/) article on the Red Blob Games website. This was my semestral work for lecture Algorithms and Data Structures II at [MFF UK](https://www.mff.cuni.cz/to.en/). 5 | 6 | The project is separated into 2 subprojects: *visibility* (header-only library) and *tests*. 7 | 8 | ## Main idea of the algorithm 9 | 10 | - Input: set of line segments (obstacles) and a position of an observer. 11 | - Output: visibility polygon (i.e. largest polygon s.t. a line segment from the observer's position to any point in the polygon does not intersect any obstacle) 12 | 13 | First, we sort endpoints of the line segments (obstacles) in CW order where the observer's position is the center point. Line segment endpoint whose other endpoint is greater in this CW order is called a *start vertex*. Otherwise it's an *end vertex*. 14 | 15 | Visibility polygon is found using a sweep line algorithm. The sweep line here is a ray. Its origin is at the observer's position and it rotates around this point. The algorithm keeps a state which is a set of line segments that are currently intersected by the ray. In addition, these line segments are sorted by the distance to the origin. When the sweep line passes through an enpoint of a line segment, it either adds this line segment to the state (in case of a *start vertex*) or removes it from the state (in case of an *end vertex*). Whenever the nearest line segment changes, we update vertices of the visibility polygon accordingly. 16 | 17 | ## API 18 | 19 | All functions and classes are in the `geometry` namespace. 20 | 21 | ### Visibility Polygon 22 | 23 | Main function of the library is the `visibility_polygon` function. It takes observer's position and begin and end iterator of the line segment list (obstacle list) as its arguments. It computes vertices of a visibility polygon in CW order and returns them. 24 | 25 | **Preconditions:** 26 | - The line segments must not intersect except at their endpoints 27 | - The visiblity polygon has to be closed. 28 | 29 | **Behaviour of the library is undefined if the preconditions aren't met**. The first condition can be met by finding all intersection points of line segments and splitting them up. The second condition can be met by adding line segments of the bounding box of all obstacles. Note: checking these conditions is entirely up you. This library does not check them as that would introduce additional overhead. 30 | 31 | ### Vector 32 | 33 | The program implements a 2D vector template in the `vector2.hpp` header. You can use immutable operators `+`, `-`, `*`, `/` as well as their mutable variants. Apart from that you can use global functions `dot(a, b)` (calculates a dot product of 2 vectors), `length_squared(vector)`, `distance_squared(a, b)`, `normal(a)` (calculates a 2D orthogonal vector), `cross(a, b)` (determinat of the `[[a_x, b_x], [a_y, b_y]]` matrix). Floating point vectors can be normalized to have an unit length using the `normalize(vector)` function (it returns 0 vector in case of a 0 vector). 34 | 35 | ### Primitives 36 | 37 | All primitive objects that are used, are defined in the `primitives.hpp` header. It defines `line_segment` and `ray` types as well as a healper function `compute_orientation` that returns orientation of 3 points in a plane (left_turn, right_turn or collinear). 38 | -------------------------------------------------------------------------------- /tests/main.cpp: -------------------------------------------------------------------------------- 1 | #define CATCH_CONFIG_MAIN 2 | #include "catch.hpp" 3 | -------------------------------------------------------------------------------- /tests/primitives_test.cpp: -------------------------------------------------------------------------------- 1 | #include "catch.hpp" 2 | 3 | #include 4 | 5 | using ray_type = geometry::ray; 6 | using segment_type = geometry::line_segment; 7 | 8 | TEST_CASE("Calculate orientation of 3 points in a plane", "[primitives]") 9 | { 10 | using namespace geometry; 11 | 12 | REQUIRE(compute_orientation(vec2{ 0, 0 }, vec2{ 1, 0 }, vec2{ 2, 1 }) == orientation::left_turn); 13 | REQUIRE(compute_orientation(vec2{ 0, 0 }, vec2{ 1, 0 }, vec2{ 2, -1 }) == orientation::right_turn); 14 | REQUIRE(compute_orientation(vec2{ 0, 0 }, vec2{ 1, 0 }, vec2{ 2, 0 }) == orientation::collinear); 15 | REQUIRE(compute_orientation(vec2{ 0, 0 }, vec2{ 0, 0 }, vec2{ 4, 5 }) == orientation::collinear); 16 | REQUIRE(compute_orientation(vec2{ 0, 0 }, vec2{ 0, 0 }, vec2{ 0, 0 }) == orientation::collinear); 17 | } 18 | 19 | TEST_CASE("Calculate an intersection point of ray and line segment", "[primitives]") 20 | { 21 | using namespace geometry; 22 | 23 | vec2 point; 24 | auto intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 25 | .intersects(segment_type{ vec2{ -1, 1 }, vec2{ -1, -1 } }, point); 26 | REQUIRE_FALSE(intersects); 27 | 28 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 29 | .intersects(segment_type{ vec2{ -1e-3f, 1 }, vec2{ -1e-3f, -1 } }, point); 30 | REQUIRE_FALSE(intersects); 31 | 32 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 33 | .intersects(segment_type{ vec2{ -2, 0 }, vec2{ -1, 0 } }, point); 34 | REQUIRE_FALSE(intersects); 35 | 36 | // ray intersects the line segment in its origin 37 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 38 | .intersects(segment_type{ vec2{ 0, 1 }, vec2{ 0, -1 } }, point); 39 | REQUIRE(intersects); 40 | REQUIRE(point.x == 0); 41 | REQUIRE(point.y == 0); 42 | 43 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 44 | .intersects(segment_type{ vec2{ -1, 0 }, vec2{ 0, 0 } }, point); 45 | REQUIRE(intersects); 46 | REQUIRE(point.x == 0); 47 | REQUIRE(point.y == 0); 48 | 49 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 50 | .intersects(segment_type{ vec2{ 0, 0 }, vec2{ -1, 0 } }, point); 51 | REQUIRE(intersects); 52 | REQUIRE(point.x == 0); 53 | REQUIRE(point.y == 0); 54 | 55 | // ray intersects the line segment in the middle 56 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 57 | .intersects(segment_type{ vec2{ 2, 1 }, vec2{ 2, -1 } }, point); 58 | REQUIRE(intersects); 59 | REQUIRE(point.x == 2); 60 | REQUIRE(point.y == 0); 61 | 62 | // ray intersects the line segment in one of its endpoints 63 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 64 | .intersects(segment_type{ vec2{ 2, 0 }, vec2{ 3, 0 } }, point); 65 | REQUIRE(intersects); 66 | REQUIRE(point.x == 2); 67 | REQUIRE(point.y == 0); 68 | 69 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 70 | .intersects(segment_type{ vec2{ 3, 0 }, vec2{ 2, 0 } }, point); 71 | REQUIRE(intersects); 72 | REQUIRE(point.x == 2); 73 | REQUIRE(point.y == 0); 74 | 75 | intersects = ray_type{ vec2{ 0, 0 }, vec2{ 1, 0 } } 76 | .intersects(segment_type{ vec2{ 1, 0 }, vec2{ 1, -1 } }, point); 77 | REQUIRE(intersects); 78 | REQUIRE(point.x == 1); 79 | REQUIRE(point.y == 0); 80 | } -------------------------------------------------------------------------------- /tests/vector2_test.cpp: -------------------------------------------------------------------------------- 1 | #include "catch.hpp" 2 | 3 | #include 4 | 5 | TEST_CASE("Create vector", "[vector]") 6 | { 7 | using namespace geometry; 8 | 9 | vec2 result(1); 10 | REQUIRE(result.x == 1); 11 | REQUIRE(result.y == 1); 12 | 13 | result = vec2{ 3, 4 }; 14 | REQUIRE(result.x == 3); 15 | REQUIRE(result.y == 4); 16 | } 17 | 18 | TEST_CASE("Add vectors", "[vector]") 19 | { 20 | using namespace geometry; 21 | 22 | vec2 a{ 1, 2 }, b{ 3, 4 }; 23 | REQUIRE((a + b).x == 4); 24 | REQUIRE((a + b).y == 6); 25 | 26 | a += b; 27 | REQUIRE(a.x == 4); 28 | REQUIRE(a.y == 6); 29 | } 30 | 31 | TEST_CASE("Subtract vectors", "[vector]") 32 | { 33 | using namespace geometry; 34 | 35 | vec2 a{ 1, 2 }, b{ 3, 4 }; 36 | REQUIRE((a - b).x == -2); 37 | REQUIRE((a - b).y == -2); 38 | 39 | a -= b; 40 | REQUIRE(a.x == -2); 41 | REQUIRE(a.y == -2); 42 | } 43 | 44 | TEST_CASE("Multiply vector", "[vector]") 45 | { 46 | using namespace geometry; 47 | 48 | vec2 a{ 1, 2 }; 49 | REQUIRE((a * 3 ).x == 3); 50 | REQUIRE((a * 3 ).y == 6); 51 | 52 | a *= 3; 53 | REQUIRE(a.x == 3); 54 | REQUIRE(a.y == 6); 55 | } 56 | 57 | TEST_CASE("Divide vector", "[vector]") 58 | { 59 | using namespace geometry; 60 | 61 | vec2 a{ 2, 8 }; 62 | REQUIRE((a / 2).x == 1); 63 | REQUIRE((a / 2).y == 4); 64 | 65 | a /= 2; 66 | REQUIRE(a.x == 1); 67 | REQUIRE(a.y == 4); 68 | } 69 | 70 | TEST_CASE("Negate vector", "[vector]") 71 | { 72 | using namespace geometry; 73 | 74 | vec2 a{ 2, 8 }; 75 | REQUIRE((-a).x == -2); 76 | REQUIRE((-a).y == -8); 77 | } 78 | 79 | TEST_CASE("Compare 2 vectors", "[vector]") 80 | { 81 | using namespace geometry; 82 | 83 | vec2 a{ 1, 2 }, b{ 1, 2 }; 84 | REQUIRE(a == b); 85 | REQUIRE_FALSE(a != b); 86 | 87 | b.x = 0; 88 | REQUIRE_FALSE(a == b); 89 | REQUIRE(a != b); 90 | } 91 | 92 | TEST_CASE("Calculate a dot product", "[vector]") 93 | { 94 | using namespace geometry; 95 | 96 | REQUIRE(dot(vec2{ 1, 2 }, vec2{ 3, 4 }) == 11); 97 | REQUIRE(dot(vec2{ 1, 2 }, vec2{ 0, 0 }) == 0); 98 | } 99 | 100 | TEST_CASE("Calculate squared length of a vector", "[vector]") 101 | { 102 | using namespace geometry; 103 | 104 | REQUIRE(length_squared(vec2{ 3, 4 }) == 25); 105 | REQUIRE(length_squared(vec2{ 0, 0 }) == 0); 106 | } 107 | 108 | TEST_CASE("Calculate square of an euclidean distance of 2 points", "[vector]") 109 | { 110 | using namespace geometry; 111 | 112 | REQUIRE(distance_squared(vec2{ 3, 4 }, vec2{ 0, 1 }) == 18); 113 | REQUIRE(distance_squared(vec2{ 3, 4 }, vec2{ 3, 4 }) == 0); 114 | } 115 | 116 | TEST_CASE("Calculate a 2D normal vector", "[vector]") 117 | { 118 | using namespace geometry; 119 | 120 | vec2 a{ 3, 4 }; 121 | auto perp = normal(a); 122 | REQUIRE(perp.x == -4); 123 | REQUIRE(perp.y == 3); 124 | } 125 | 126 | TEST_CASE("Calculate a determinant", "[vector]") 127 | { 128 | using namespace geometry; 129 | 130 | vec2 a{ 3, 4 }, b{ 1, 2}; 131 | auto det = cross(a, b); 132 | REQUIRE(det == 2); 133 | } 134 | 135 | TEST_CASE("Normalize a floating point vector", "[vector]") 136 | { 137 | using namespace geometry; 138 | 139 | vec2 a{ 3, 4 }; 140 | auto normalized = normalize(a); 141 | REQUIRE(length_squared(normalized) == Approx(1)); 142 | 143 | vec2 zero{ 0, 0 }; 144 | normalized = normalize(zero); 145 | REQUIRE(normalized.x == 0); 146 | REQUIRE(normalized.y == 0); 147 | } -------------------------------------------------------------------------------- /tests/visibility_test.cpp: -------------------------------------------------------------------------------- 1 | #include "catch.hpp" 2 | 3 | #include 4 | 5 | using vector_type = geometry::vec2; 6 | using segment_type = geometry::line_segment; 7 | using segment_comparer_type = geometry::line_segment_dist_comparer; 8 | using angle_comparer_type = geometry::angle_comparer; 9 | 10 | void test_line_segment_is_closer( 11 | const segment_comparer_type& cmp, 12 | vector_type a, vector_type b, 13 | vector_type c, vector_type d) 14 | { 15 | REQUIRE(cmp(segment_type{ a, b }, { c, d })); 16 | REQUIRE(cmp(segment_type{ b, a }, { c, d })); 17 | REQUIRE(cmp(segment_type{ a, b }, { d, c })); 18 | REQUIRE(cmp(segment_type{ b, a }, { d, c })); 19 | 20 | REQUIRE_FALSE(cmp(segment_type{ c, d }, { a, b })); 21 | REQUIRE_FALSE(cmp(segment_type{ d, c }, { a, b })); 22 | REQUIRE_FALSE(cmp(segment_type{ c, d }, { b, a })); 23 | REQUIRE_FALSE(cmp(segment_type{ d, c }, { b, a })); 24 | } 25 | 26 | void test_line_segments_are_equal( 27 | const segment_comparer_type& cmp, 28 | vector_type a, vector_type b, 29 | vector_type c, vector_type d) 30 | { 31 | REQUIRE_FALSE(cmp(segment_type{ a, b }, { c, d })); 32 | REQUIRE_FALSE(cmp(segment_type{ b, a }, { c, d })); 33 | REQUIRE_FALSE(cmp(segment_type{ a, b }, { d, c })); 34 | REQUIRE_FALSE(cmp(segment_type{ b, a }, { d, c })); 35 | 36 | REQUIRE_FALSE(cmp(segment_type{ c, d }, { a, b })); 37 | REQUIRE_FALSE(cmp(segment_type{ d, c }, { a, b })); 38 | REQUIRE_FALSE(cmp(segment_type{ c, d }, { b, a })); 39 | REQUIRE_FALSE(cmp(segment_type{ d, c }, { b, a })); 40 | } 41 | 42 | TEST_CASE("Compare 2 line segments with no common endpoints", "[visibility][dist_comp]") 43 | { 44 | segment_comparer_type cmp{ { 0, 0 } }; 45 | 46 | test_line_segment_is_closer(cmp, { 1, 1 }, { 1, -1 }, { 2, 1 }, { 2, -1 }); 47 | test_line_segment_is_closer(cmp, { 1, 1 }, { 1, -1 }, { 2, 2 }, { 2, 3 }); 48 | } 49 | 50 | TEST_CASE("Compare 2 line segments with common endpoints", "[visibility][dist_comp]") 51 | { 52 | segment_comparer_type cmp{ { 0, 0 } }; 53 | 54 | test_line_segments_are_equal(cmp, { 1, 1 }, { 1, 0 }, { 1, 0 }, { 1, -1 }); 55 | test_line_segments_are_equal(cmp, { 1, 1 }, { 1, 0 }, { 1, 0 }, { 1, 1 }); 56 | test_line_segment_is_closer(cmp, { 2, 0 }, { 1, 1 }, { 2, 1 }, { 2, 0 }); 57 | test_line_segment_is_closer(cmp, { 2, 1 }, { 2, 0 }, { 2, 0 }, { 3, 1 }); 58 | } 59 | 60 | TEST_CASE("Compare angle with 2 points in general position", "[visibility][angle_comp]") 61 | { 62 | angle_comparer_type cmp{ { 0, 0 } }; 63 | 64 | REQUIRE(cmp({ 0, 1 }, { 1, 1 })); 65 | REQUIRE_FALSE(cmp({ 1, 1 }, { 0, 1 })); 66 | 67 | REQUIRE(cmp({ 1, 1 }, { 1, -1 })); 68 | REQUIRE_FALSE(cmp({ 1, -1 }, { 1, 1 })); 69 | 70 | REQUIRE(cmp({ 1, 0 }, { -1, -1 })); 71 | REQUIRE_FALSE(cmp({ -1, -1 }, { 1, 0 })); 72 | 73 | REQUIRE(cmp({ 0, 1 }, { 0, -1 })); 74 | REQUIRE_FALSE(cmp({ 0, -1 }, { 0, 1 })); 75 | } 76 | 77 | TEST_CASE("Compare angle with 2 points if they are collinear with the origin", "[visibility][angle_comp]") 78 | { 79 | angle_comparer_type cmp{ { 0, 0 } }; 80 | 81 | REQUIRE(cmp({ 1, 0 }, { 2, 0 })); 82 | REQUIRE_FALSE(cmp({ 2, 0 }, { 1, 0 })); 83 | 84 | REQUIRE_FALSE(cmp({ 1, 0 }, { 1, 0 })); 85 | REQUIRE_FALSE(cmp({ 0, 0 }, { 0, 0 })); 86 | } 87 | 88 | TEST_CASE("Calculate visibility polygon with no line segments", "[visibility]") 89 | { 90 | std::vector segments; 91 | auto poly = geometry::visibility_polygon(vector_type{ 0, 0 }, segments.begin(), segments.end()); 92 | REQUIRE(poly.size() == 0); 93 | } 94 | 95 | TEST_CASE("Calculate visibility polygon with no obstaces apart from the boundary", "[visibility]") 96 | { 97 | using namespace geometry; 98 | std::vector segments{ 99 | { { -250, -250 },{ -250, 250 } }, 100 | { { -250, 250 },{ 250, 250 } }, 101 | { { 250, 250 },{ 250, -250 } }, 102 | { { 250, -250 },{ -250, -250 } } 103 | }; 104 | 105 | auto poly = visibility_polygon(vector_type{ 0, 0 }, segments.begin(), segments.end()); 106 | REQUIRE(poly.size() == 4); 107 | REQUIRE(approx_equal(poly[0], { 250, 250 })); 108 | REQUIRE(approx_equal(poly[1], { 250, -250 })); 109 | REQUIRE(approx_equal(poly[2], { -250, -250 })); 110 | REQUIRE(approx_equal(poly[3], { -250, 250 })); 111 | } 112 | 113 | TEST_CASE("Calculate visibility polygon with a polyline as an obstacle", "[visibility]") 114 | { 115 | using namespace geometry; 116 | std::vector segments{ 117 | { { -250, -250 },{ -250, 250 } }, 118 | { { -250, 250 },{ 250, 250 } }, 119 | { { 250, 250 },{ 250, -250 } }, 120 | { { 250, -250 },{ -250, -250 } }, 121 | 122 | { { -50, 50 },{ 50, 50 } }, 123 | { { 50, 50 },{ 50, -50 } }, 124 | }; 125 | 126 | auto poly = visibility_polygon(vector_type{ 0, 0 }, segments.begin(), segments.end()); 127 | REQUIRE(poly.size() == 6); 128 | REQUIRE(approx_equal(poly[0], { 50, 50 })); 129 | REQUIRE(approx_equal(poly[1], { 50, -50 })); 130 | REQUIRE(approx_equal(poly[2], { 250, -250 })); 131 | REQUIRE(approx_equal(poly[3], { -250, -250 })); 132 | REQUIRE(approx_equal(poly[4], { -250, 250 })); 133 | REQUIRE(approx_equal(poly[5], { -50, 50 })); 134 | } 135 | 136 | TEST_CASE("Calculate visibility polygon with a convex polygon as an obstacle", "[visibility]") 137 | { 138 | using namespace geometry; 139 | std::vector segments{ 140 | { { -250, -250 },{ -250, 250 } }, 141 | { { -250, 250 },{ 250, 250 } }, 142 | { { 250, 250 },{ 250, -250 } }, 143 | { { 250, -250 },{ -250, -250 } }, 144 | 145 | { { -50, 50 }, { 50, 50 } }, 146 | { { 50, 50 }, { 50, 100 } }, 147 | { { 50, 100 }, { -50, 100 } }, 148 | { { -50, 100 }, { -50, 50 } }, 149 | }; 150 | 151 | auto poly = visibility_polygon(vector_type{ 0, 0 }, segments.begin(), segments.end()); 152 | REQUIRE(poly.size() == 6); 153 | REQUIRE(approx_equal(poly[0], { 50, 50 })); 154 | REQUIRE(approx_equal(poly[1], { 250, 250 })); 155 | REQUIRE(approx_equal(poly[2], { 250, -250 })); 156 | REQUIRE(approx_equal(poly[3], { -250, -250 })); 157 | REQUIRE(approx_equal(poly[4], { -250, 250 })); 158 | REQUIRE(approx_equal(poly[5], { -50, 50 })); 159 | } 160 | 161 | TEST_CASE("Calculate visibility polygon with a concave polygon as an obstacle", "[visibility]") 162 | { 163 | using namespace geometry; 164 | std::vector segments{ 165 | { { -250, -250 },{ -250, 250 } }, 166 | { { -250, 250 },{ 250, 250 } }, 167 | { { 250, 250 },{ 250, -250 } }, 168 | { { 250, -250 },{ -250, -250 } }, 169 | 170 | { { -50, 50 },{ 0, 100 } }, 171 | { { 0, 100 }, { 50, 50 } }, 172 | { { 50, 50 },{ 0, 200 } }, 173 | { { 0, 200 },{ -50, 50 } }, 174 | }; 175 | 176 | auto poly = visibility_polygon(vector_type{ 0, 0 }, segments.begin(), segments.end()); 177 | REQUIRE(poly.size() == 7); 178 | REQUIRE(approx_equal(poly[0], { 0, 100 })); 179 | REQUIRE(approx_equal(poly[1], { 50, 50 })); 180 | REQUIRE(approx_equal(poly[2], { 250, 250 })); 181 | REQUIRE(approx_equal(poly[3], { 250, -250 })); 182 | REQUIRE(approx_equal(poly[4], { -250, -250 })); 183 | REQUIRE(approx_equal(poly[5], { -250, 250 })); 184 | REQUIRE(approx_equal(poly[6], { -50, 50 })); 185 | } -------------------------------------------------------------------------------- /visibility/floats.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_FLOATS_HPP_ 2 | #define GEOMETRY_FLOATS_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "vector2.hpp" 8 | 9 | namespace geometry 10 | { 11 | inline bool approx_equal(float a, float b, float epsilon = std::numeric_limits::epsilon()) 12 | { 13 | return std::abs(a - b) <= std::max(std::abs(a), std::abs(b)) * epsilon; 14 | } 15 | 16 | inline bool strictly_less(float a, float b, float epsilon = std::numeric_limits::epsilon()) 17 | { 18 | return (b - a) > std::max(std::abs(a), std::abs(b)) * epsilon; 19 | } 20 | 21 | template 22 | bool approx_equal(vector2 a, vector2 b, T epsilon = std::numeric_limits::epsilon()) 23 | { 24 | return approx_equal(a.x, b.x, epsilon) && 25 | approx_equal(a.y, b.y, epsilon); 26 | } 27 | 28 | template 29 | bool strictly_less(vector2& a, vector2 b, T epsilon = std::numeric_limits::epsilon()) 30 | { 31 | return strictly_less(a.x, b.x, epsilon) && 32 | strictly_less(a.y, b.y, epsilon); 33 | } 34 | } 35 | 36 | #endif // GEOMETRY_FLOATS_HPP_ -------------------------------------------------------------------------------- /visibility/primitives.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_PRIMITIVES_HPP_ 2 | #define GEOMETRY_PRIMITIVES_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "vector2.hpp" 8 | #include "floats.hpp" 9 | 10 | namespace geometry 11 | { 12 | enum class orientation 13 | { 14 | left_turn = 1, 15 | right_turn = -1, 16 | collinear = 0 17 | }; 18 | 19 | /** Compute orientation of 3 points in a plane. 20 | * @param a first point 21 | * @param b second point 22 | * @param c third point 23 | * @return orientation of the points in the plane (left turn, right turn 24 | * or collinear) 25 | */ 26 | template 27 | orientation compute_orientation(Vector a, Vector b, Vector c) 28 | { 29 | auto det = cross(b - a, c - a); 30 | return static_cast( 31 | static_cast(strictly_less(0.f, det)) - 32 | static_cast(strictly_less(det, 0.f)) 33 | ); 34 | } 35 | 36 | template 37 | struct line_segment 38 | { 39 | Vector a, b; 40 | 41 | line_segment() {} 42 | line_segment(Vector a, Vector b) : a(a), b(b) {} 43 | line_segment(const line_segment&) = default; 44 | line_segment& operator=(const line_segment& segment) = default; 45 | }; 46 | 47 | template 48 | struct ray 49 | { 50 | Vector origin; 51 | Vector direction; 52 | 53 | ray() {} 54 | ray(Vector origin, Vector direction) : 55 | origin(origin), 56 | direction(direction) {} 57 | 58 | /** Find the nearest intersection point of ray and line segment. 59 | * @param segment 60 | * @param out_point reference to a variable where the nearest 61 | * intersection point will be stored (can be changed even 62 | * when there is no intersection) 63 | * @return true iff the ray intersects the line segment 64 | */ 65 | bool intersects( 66 | const line_segment& segment, 67 | Vector& out_point) const 68 | { 69 | auto ao = origin - segment.a; 70 | auto ab = segment.b - segment.a; 71 | auto det = cross(ab, direction); 72 | if (approx_equal(det, 0.f)) 73 | { 74 | auto abo = compute_orientation(segment.a, segment.b, origin); 75 | if (abo != orientation::collinear) 76 | return false; 77 | auto dist_a = dot(ao, direction); 78 | auto dist_b = dot(origin - segment.b, direction); 79 | 80 | if (dist_a > 0 && dist_b > 0) 81 | return false; 82 | else if ((dist_a > 0) != (dist_b > 0)) 83 | out_point = origin; 84 | else if (dist_a > dist_b) // at this point, both distances are negative 85 | out_point = segment.a; // hence the nearest point is A 86 | else 87 | out_point = segment.b; 88 | return true; 89 | } 90 | 91 | auto u = cross(ao, direction) / det; 92 | if (strictly_less(u, 0.f) || 93 | strictly_less(1.f, u)) 94 | return false; 95 | 96 | auto t = -cross(ab, ao) / det; 97 | out_point = origin + t * direction; 98 | return approx_equal(t, 0.f) || t > 0; 99 | } 100 | }; 101 | } 102 | 103 | #endif // GEOMETRY_PRIMITIVES_HPP_ -------------------------------------------------------------------------------- /visibility/vector2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_VECTOR2_HPP_ 2 | #define GEOMETRY_VECTOR2_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace geometry 14 | { 15 | // Simple 2d vector type 16 | template 17 | struct vector2 18 | { 19 | template 20 | using if_vector = typename std::enable_if< 21 | !std::is_scalar::value, ReturnType>::type; 22 | 23 | template 24 | using if_scalar = typename std::enable_if< 25 | std::is_scalar::value, ReturnType>::type; 26 | 27 | struct { T x, y; }; 28 | 29 | vector2() {} 30 | vector2(T x, T y) : x(x), y(y) {} 31 | explicit vector2(const T& scalar) : x(scalar), y(scalar) {} 32 | 33 | // allow copy 34 | vector2(const vector2&) = default; 35 | vector2& operator=(const vector2&) = default; 36 | 37 | // Mutable operations 38 | template 39 | vector2& operator+=(VectorType other) 40 | { 41 | x += other.x; 42 | y += other.y; 43 | return *this; 44 | } 45 | 46 | template 47 | vector2& operator-=(VectorType other) 48 | { 49 | x -= other.x; 50 | y -= other.y; 51 | return *this; 52 | } 53 | 54 | template 55 | if_vector operator*=(VectorType other) 56 | { 57 | x *= other.x; 58 | y *= other.y; 59 | return *this; 60 | } 61 | 62 | template 63 | if_vector operator/=(VectorType other) 64 | { 65 | x /= other.x; 66 | y /= other.y; 67 | return *this; 68 | } 69 | 70 | template 71 | if_scalar operator*=(ScalarType scalar) 72 | { 73 | x *= scalar; 74 | y *= scalar; 75 | return *this; 76 | } 77 | 78 | template 79 | if_scalar operator/=(ScalarType scalar) 80 | { 81 | x /= scalar; 82 | y /= scalar; 83 | return *this; 84 | } 85 | 86 | // immutable operations 87 | template 88 | vector2 operator+(VectorType other) const 89 | { 90 | return { x + other.x, y + other.y }; 91 | } 92 | 93 | template 94 | vector2 operator-(VectorType other) const 95 | { 96 | return { x - other.x, y - other.y }; 97 | } 98 | 99 | template 100 | if_vector operator*(VectorType other) const 101 | { 102 | return { x * other.x, y * other.y }; 103 | } 104 | 105 | template 106 | if_vector operator/(VectorType other) const 107 | { 108 | return { x / other.x, y / other.y }; 109 | } 110 | 111 | template 112 | if_scalar operator*(ScalarType scalar) const 113 | { 114 | return { x * scalar, y * scalar }; 115 | } 116 | 117 | template 118 | friend if_scalar operator*( 119 | ScalarType scalar, 120 | vector2 vector) 121 | { 122 | return { vector.x * scalar, vector.y * scalar }; 123 | } 124 | 125 | template 126 | if_scalar operator/(ScalarType scalar) const 127 | { 128 | return { x / scalar, y / scalar }; 129 | } 130 | 131 | friend vector2 operator-(vector2 vec) 132 | { 133 | return { -vec.x, -vec.y }; 134 | } 135 | 136 | // comparison operators 137 | template 138 | bool operator==(VectorType other) const 139 | { 140 | return x == other.x && y == other.y; 141 | } 142 | 143 | template 144 | bool operator!=(VectorType other) const 145 | { 146 | return x != other.x || y != other.y; 147 | } 148 | }; 149 | 150 | /** Calculate standard dot product. 151 | * @param a vector 152 | * @param b vector 153 | * @return dot product of the 2 vectors 154 | */ 155 | template 156 | auto dot(Vector a, Vector b) 157 | { 158 | return a.x * b.x + a.y * b.y; 159 | } 160 | 161 | /** Calculate squared length of a vector. 162 | * @param vector 163 | * @return squared length of the vector 164 | */ 165 | template 166 | auto length_squared(Vector vector) 167 | { 168 | return dot(vector, vector); 169 | } 170 | 171 | /** Squared distance of 2 points. 172 | * @param a point 173 | * @param b point 174 | * @return squared distance of the 2 points 175 | */ 176 | template 177 | auto distance_squared(Vector a, Vector b) 178 | { 179 | return length_squared(a - b); 180 | } 181 | 182 | /** Return orthogonal 2D vector. 183 | * @param vector 184 | * @return vector orthogonal to the argument 185 | */ 186 | template 187 | Vector normal(Vector vector) 188 | { 189 | return{ -vector.y, vector.x }; 190 | } 191 | 192 | /** Calculate det([a_x, b.x; a_y, b_y]). 193 | * @param a vector 194 | * @param b vector 195 | * @return det([a_x, b.x; a_y, b_y]) 196 | */ 197 | template 198 | auto cross(Vector a, Vector b) 199 | { 200 | return a.x * b.y - a.y * b.x; 201 | } 202 | 203 | /** Normalize a floating point vector to have an unit length. 204 | * @param vector to normalize 205 | * @return normalized vector. 206 | * If the vector is 0, it will return a 0 vector. 207 | * If the vector is non-zero, it will return a vector with the same 208 | * direction and unit length. 209 | */ 210 | template 211 | Vector normalize(Vector vector) 212 | { 213 | using value_type = typename std::decay::type; 214 | 215 | value_type length = std::sqrt(dot(vector, vector)); 216 | if (std::abs(length) < std::numeric_limits::epsilon()) 217 | return vector; 218 | vector /= length; 219 | return vector; 220 | } 221 | 222 | /** Print a vector to the output stream. 223 | * @param output stream 224 | * @param vector to print 225 | * @return reference to given output stream 226 | */ 227 | template 228 | std::ostream& operator<<(std::ostream& output, vector2 vector) 229 | { 230 | output << "[" << vector.x << ", " << vector.y << "]"; 231 | return output; 232 | } 233 | 234 | using vec2 = vector2; 235 | } 236 | 237 | #endif // GEOMETRY_VECTOR2_HPP_ -------------------------------------------------------------------------------- /visibility/visibility.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_VISIBILITY_HPP_ 2 | #define GEOMETRY_VISIBILITY_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "floats.hpp" 12 | #include "vector2.hpp" 13 | #include "primitives.hpp" 14 | 15 | namespace geometry 16 | { 17 | /* Compare 2 line segments based on their distance from given point 18 | * Assumes: (1) the line segments are intersected by some ray from the origin 19 | * (2) the line segments do not intersect except at their endpoints 20 | * (3) no line segment is collinear with the origin 21 | */ 22 | template 23 | struct line_segment_dist_comparer 24 | { 25 | using segment_type = line_segment; 26 | 27 | Vector origin; 28 | 29 | explicit line_segment_dist_comparer(Vector origin) : 30 | origin(origin) 31 | { 32 | } 33 | 34 | /** Check whether the line segment x is closer to the origin than the 35 | * line segment y. 36 | * @param x line segment: left hand side of the comparison operator 37 | * @param y line segment: right hand side of the comparison operator 38 | * @return true iff x < y (x is closer than y) 39 | */ 40 | bool operator()(const segment_type& x, const segment_type& y) const 41 | { 42 | auto a = x.a, b = x.b; 43 | auto c = y.a, d = y.b; 44 | 45 | assert( 46 | compute_orientation(origin, a, b) != orientation::collinear && 47 | "AB must not be collinear with the origin."); 48 | assert( 49 | compute_orientation(origin, c, d) != orientation::collinear && 50 | "CD must not be collinear with the origin."); 51 | 52 | // sort the endpoints so that if there are common endpoints, 53 | // it will be a and c 54 | if (approx_equal(b, c) || approx_equal(b, d)) 55 | std::swap(a, b); 56 | if (approx_equal(a, d)) 57 | std::swap(c, d); 58 | 59 | // cases with common endpoints 60 | if (approx_equal(a, c)) 61 | { 62 | auto oad = compute_orientation(origin, a, d); 63 | auto oab = compute_orientation(origin, a, b); 64 | if (approx_equal(b, d) || oad != oab) 65 | return false; 66 | return compute_orientation(a, b, d) != compute_orientation(a, b, origin); 67 | } 68 | 69 | // cases without common endpoints 70 | auto cda = compute_orientation(c, d, a); 71 | auto cdb = compute_orientation(c, d, b); 72 | if (cdb == orientation::collinear && cda == orientation::collinear) 73 | { 74 | return distance_squared(origin, a) < distance_squared(origin, c); 75 | } 76 | else if (cda == cdb || 77 | cda == orientation::collinear || 78 | cdb == orientation::collinear) 79 | { 80 | auto cdo = compute_orientation(c, d, origin); 81 | return cdo == cda || cdo == cdb; 82 | } 83 | else 84 | { 85 | auto abo = compute_orientation(a, b, origin); 86 | return abo != compute_orientation(a, b, c); 87 | } 88 | } 89 | }; 90 | 91 | // compare angles clockwise starting at the positive y axis 92 | template 93 | struct angle_comparer 94 | { 95 | Vector vertex; 96 | 97 | explicit angle_comparer(Vector origin) : vertex(origin) {} 98 | 99 | bool operator()(const Vector& a, const Vector& b) const 100 | { 101 | auto is_a_left = strictly_less(a.x, vertex.x); 102 | auto is_b_left = strictly_less(b.x, vertex.x); 103 | if (is_a_left != is_b_left) 104 | return is_b_left; 105 | 106 | if (approx_equal(a.x, vertex.x) && approx_equal(b.x, vertex.x)) 107 | { 108 | if (!strictly_less(a.y, vertex.y) || 109 | !strictly_less(b.y, vertex.y)) 110 | { 111 | return strictly_less(b.y, a.y); 112 | } 113 | return strictly_less(a.y, b.y); 114 | } 115 | 116 | auto oa = a - vertex; 117 | auto ob = b - vertex; 118 | auto det = cross(oa, ob); 119 | if (approx_equal(det, 0.f)) 120 | { 121 | return length_squared(oa) < length_squared(ob); 122 | } 123 | return det < 0; 124 | } 125 | }; 126 | 127 | template 128 | struct visibility_event 129 | { 130 | // events used in the visibility polygon algorithm 131 | enum event_type 132 | { 133 | start_vertex, 134 | end_vertex 135 | }; 136 | 137 | event_type type; 138 | line_segment segment; 139 | 140 | visibility_event() {} 141 | visibility_event(event_type type, const line_segment& segment) : 142 | type(type), 143 | segment(segment) {} 144 | 145 | const auto& point() const { return segment.a; } 146 | }; 147 | 148 | /** Calculate visibility polygon vertices in clockwise order. 149 | * Endpoints of the line segments (obstacles) can be ordered arbitrarily. 150 | * Line segments collinear with the point are ignored. 151 | * @param point - position of the observer 152 | * @param begin iterator of the list of line segments (obstacles) 153 | * @param end iterator of the list of line segments (obstacles) 154 | * @return vector of vertices of the visibility polygon 155 | */ 156 | template 157 | std::vector visibility_polygon( 158 | Vector point, 159 | InputIterator begin, 160 | InputIterator end) 161 | { 162 | using segment_type = line_segment; 163 | using event_type = visibility_event; 164 | using segment_comparer_type = line_segment_dist_comparer; 165 | 166 | segment_comparer_type cmp_dist{ point }; 167 | std::set state{ cmp_dist }; 168 | std::vector events; 169 | 170 | for (; begin != end; ++begin) 171 | { 172 | auto segment = *begin; 173 | 174 | // Sort line segment endpoints and add them as events 175 | // Skip line segments collinear with the point 176 | auto pab = compute_orientation(point, segment.a, segment.b); 177 | if (pab == orientation::collinear) 178 | { 179 | continue; 180 | } 181 | else if (pab == orientation::right_turn) 182 | { 183 | events.emplace_back(event_type::start_vertex, segment); 184 | events.emplace_back( 185 | event_type::end_vertex, 186 | segment_type{ segment.b, segment.a }); 187 | } 188 | else 189 | { 190 | events.emplace_back( 191 | event_type::start_vertex, 192 | segment_type{ segment.b, segment.a }); 193 | events.emplace_back(event_type::end_vertex, segment); 194 | } 195 | 196 | // Initialize state by adding line segments that are intersected 197 | // by vertical ray from the point 198 | auto a = segment.a, b = segment.b; 199 | if (a.x > b.x) 200 | std::swap(a, b); 201 | 202 | auto abp = compute_orientation(a, b, point); 203 | if (abp == orientation::right_turn && 204 | (approx_equal(b.x, point.x) || 205 | (a.x < point.x && point.x < b.x))) 206 | { 207 | state.insert(segment); 208 | } 209 | } 210 | 211 | // sort events by angle 212 | angle_comparer cmp_angle{ point }; 213 | std::sort(events.begin(), events.end(), [&cmp_angle](auto&& a, auto&& b) 214 | { 215 | // if the points are equal, sort end vertices first 216 | if (approx_equal(a.point(), b.point())) 217 | return a.type == event_type::end_vertex && 218 | b.type == event_type::start_vertex; 219 | return cmp_angle(a.point(), b.point()); 220 | }); 221 | 222 | // find the visibility polygon 223 | std::vector vertices; 224 | for (auto&& event : events) 225 | { 226 | if (event.type == event_type::end_vertex) 227 | state.erase(event.segment); 228 | 229 | if (state.empty()) 230 | { 231 | vertices.push_back(event.point()); 232 | } 233 | else if (cmp_dist(event.segment, *state.begin())) 234 | { 235 | // Nearest line segment has changed 236 | // Compute the intersection point with this segment 237 | vec2 intersection; 238 | ray ray{ point, event.point() - point }; 239 | auto nearest_segment = *state.begin(); 240 | auto intersects = ray.intersects(nearest_segment, intersection); 241 | assert(intersects && 242 | "Ray intersects line segment L iff L is in the state"); 243 | 244 | if (event.type == event_type::start_vertex) 245 | { 246 | vertices.push_back(intersection); 247 | vertices.push_back(event.point()); 248 | } 249 | else 250 | { 251 | vertices.push_back(event.point()); 252 | vertices.push_back(intersection); 253 | } 254 | } 255 | 256 | if (event.type == event_type::start_vertex) 257 | state.insert(event.segment); 258 | } 259 | 260 | // remove collinear points 261 | auto top = vertices.begin(); 262 | for (auto it = vertices.begin(); it != vertices.end(); ++it) 263 | { 264 | auto prev = top == vertices.begin() ? vertices.end() - 1 : top - 1; 265 | auto next = it + 1 == vertices.end() ? vertices.begin() : it + 1; 266 | if (compute_orientation(*prev, *it, *next) != orientation::collinear) 267 | *top++ = *it; 268 | } 269 | vertices.erase(top, vertices.end()); 270 | return vertices; 271 | } 272 | } 273 | 274 | #endif // GEOMETRY_VISIBILITY_HPP_ --------------------------------------------------------------------------------