diff --git a/include/boost/geometry/algorithms/detail/get_left_turns.hpp b/include/boost/geometry/algorithms/detail/get_left_turns.hpp index 4f127b255..8f99fa347 100644 --- a/include/boost/geometry/algorithms/detail/get_left_turns.hpp +++ b/include/boost/geometry/algorithms/detail/get_left_turns.hpp @@ -1,6 +1,6 @@ // Boost.Geometry (aka GGL, Generic Geometry Library) -// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands. +// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands. // Use, modification and distribution is subject to the Boost Software License, // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at @@ -9,7 +9,12 @@ #ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_GET_LEFT_TURNS_HPP #define BOOST_GEOMETRY_ALGORITHMS_DETAIL_GET_LEFT_TURNS_HPP +#include +#include +#include +#include #include +#include namespace boost { namespace geometry { @@ -19,6 +24,7 @@ namespace boost { namespace geometry namespace detail { +#ifdef BOOST_GEOMETRY_OLD_GET_LEFT_TURNS // TODO: move this to /util/ template static inline std::pair ordered_pair(T const& first, T const& second) @@ -361,6 +367,239 @@ inline void calculate_left_turns(Angles const& angles, prefer_by_priority(turns, keep_indices); } } +#else + +namespace left_turns +{ + +template +struct turn_angle_info +{ + bg::segment_identifier seg_id; + Point points[2]; + + turn_angle_info(bg::segment_identifier const& id, Point const& from, Point const& to) + : seg_id(id) + { + points[0] = from; + points[1] = to; + } +}; + + +template +struct angle_info +{ + bg::segment_identifier seg_id; + Point point; + bool incoming; + bool blocked; + + inline angle_info(bg::segment_identifier const& id, bool inc, Point const& p) + : seg_id(id) + , point(p) + , incoming(inc) + , blocked(false) + { + } +}; + +template +inline int get_quadrant(Vector const& vector) +{ + // Return quadrant as layouted in the code below: + // 3 | 0 + // ----- + // 2 | 1 + return geometry::get<1>(vector) >= 0 + ? (geometry::get<0>(vector) < 0 ? 3 : 0) + : (geometry::get<0>(vector) < 0 ? 2 : 1) + ; +} + + +template +struct angle_less +{ + typedef Point vector_type; + typedef typename strategy::side::services::default_strategy + < + typename cs_tag::type + >::type side_strategy_type; + + angle_less(Point const& origin) + : m_origin(origin) + {} + + inline int length(vector_type const& v) const + { + return geometry::get<0>(v) * geometry::get<0>(v) + + geometry::get<1>(v) * geometry::get<1>(v) + ; + } + + template + inline bool operator()(Angle const& p, Angle const& q) const + { + // Vector origin -> p and origin -> q + vector_type pv = p.point; + vector_type qv = q.point; + geometry::subtract_point(pv, m_origin); + geometry::subtract_point(qv, m_origin); + + int const quadrant_p = get_quadrant(pv); + int const quadrant_q = get_quadrant(qv); + if (quadrant_p != quadrant_q) + { + return quadrant_p < quadrant_q; + } + // Same quadrant, check if p is located left of q + int const side = side_strategy_type::apply(m_origin, q.point, + p.point); + if (side != 0) + { + return side == 1; + } + // Collinear, check if one is incoming + if (p.incoming != q.incoming) + { + return int(p.incoming) < int(q.incoming); + } + // Same quadrant/side/direction, return longest first + int const length_p = length(pv); + int const length_q = length(qv); + if (length_p != length_q) + { + return length(pv) > length(qv); + } + // They are still the same. Just compare on seg_id + return p.seg_id < q.seg_id; + } + +private: + Point m_origin; +}; + +template +struct angle_equal_to +{ + typedef Point vector_type; + typedef typename strategy::side::services::default_strategy + < + typename cs_tag::type + >::type side_strategy_type; + + inline angle_equal_to(Point const& origin) + : m_origin(origin) + {} + + template + inline bool operator()(Angle const& p, Angle const& q) const + { + // Vector origin -> p and origin -> q + vector_type pv = p.point; + vector_type qv = q.point; + geometry::subtract_point(pv, m_origin); + geometry::subtract_point(qv, m_origin); + + if (get_quadrant(pv) != get_quadrant(qv)) + { + return false; + } + // Same quadrant, check if p/q are collinear + int const side = side_strategy_type::apply(m_origin, q.point, + p.point); + return side == 0; + } + +private: + Point m_origin; +}; + +struct left_turn +{ + segment_identifier from; + segment_identifier to; +}; + + +template +inline void get_left_turns(AngleCollection const& sorted_angles, Point const& origin, + OutputCollection& output_collection) +{ + angle_equal_to comparator(origin); + typedef geometry::closing_iterator closing_iterator; + closing_iterator it(sorted_angles); + closing_iterator end(sorted_angles, true); + + closing_iterator previous = it++; + for( ; it != end; previous = it++) + { + if (! it->blocked) + { + bool equal = comparator(*previous, *it); + bool include = ! equal + && previous->incoming + && !it->incoming; + if (include) + { + left_turn turn; + turn.from = previous->seg_id; + turn.to = it->seg_id; + output_collection.push_back(turn); + } + } + } +} + +template +inline void block_turns_on_right_sides(AngleTurnCollection const& turns, + AngleCollection& sorted) +{ + // Create a small (seg_id -> index) map for fast finding turns + std::map incoming; + std::map outgoing; + int index = 0; + for (typename boost::range_iterator::type it = + sorted.begin(); it != sorted.end(); ++it, ++index) + { + if (it->incoming) + { + incoming[it->seg_id] = index; + } + else + { + outgoing[it->seg_id] = index; + } + } + + // Walk through turns and block every outgoing angle on the right side + for (typename boost::range_iterator::type it = + turns.begin(); it != turns.end(); ++it) + { + int incoming_index = incoming[it->seg_id]; + int outgoing_index = outgoing[it->seg_id]; + int index = incoming_index; + while(index != outgoing_index) + { + if (!sorted[index].incoming) + { + sorted[index].blocked = true; + } + + // Go back (circular) + index--; + if (index == -1) + { + index = boost::size(sorted) - 1; + } + } + } +} + +} // namespace left_turns + +#endif } // namespace detail #endif // DOXYGEN_NO_DETAIL diff --git a/test/algorithms/detail/get_left_turns.cpp b/test/algorithms/detail/get_left_turns.cpp new file mode 100644 index 000000000..4efb0df8a --- /dev/null +++ b/test/algorithms/detail/get_left_turns.cpp @@ -0,0 +1,316 @@ +// Boost.Geometry (aka GGL, Generic Geometry Library) +// +// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands. + +// Use, modification and distribution is subject to the Boost Software License, +// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at +// http://www.boost.org/LICENSE_1_0.txt) + +#include + +#include +#include + +#include +#include + +#if defined(TEST_WITH_SVG) +# include +#endif + +namespace bglt = boost::geometry::detail::left_turns; + +#if defined(TEST_WITH_SVG) +template +inline Point further_than(Point const& p, Point const& origin, int mul, int div) +{ + typedef Point vector_type; + + vector_type v = p; + bg::subtract_point(v, origin); + + bg::divide_value(v, div); + bg::multiply_value(v, mul); + Point result = origin; + bg::add_point(result, v); + return result; +} + +inline std::string get_color(int index) +{ + switch (index) + { + case 0 : return "rgb(0,192,0)"; + case 1 : return "rgb(0,0,255)"; + case 2 : return "rgb(255,0,0)"; + case 3 : return "rgb(255,255,0)"; + } + return "rgb(128,128,128)"; +} +#endif + + +template +void test_one(std::string const& caseid, + Point const& p, + std::vector > const& angles, + std::string const& expected_sorted_indices, + std::string const& expected_left_indices) +{ + typedef Point vector_type; + + std::vector > sorted; + for (typename std::vector >::const_iterator it = + angles.begin(); it != angles.end(); ++it) + { + for (int i = 0; i < 2; i++) + { + bglt::angle_info info(it->seg_id, i == 0, it->points[i]); + sorted.push_back(info); + } + } + + // Sort on angle + std::sort(sorted.begin(), sorted.end(), bglt::angle_less(p)); + + // Block all turns on the right side of any turn + bglt::block_turns_on_right_sides(angles, sorted); + + // Check the sorting + { + std::ostringstream out; + out << std::boolalpha; + for (typename std::vector >::const_iterator it = + sorted.begin(); it != sorted.end(); ++it) + { + out << " " << it->seg_id.segment_index + << "-" << it->incoming; + } + std::string detected = boost::trim_copy(out.str()); + BOOST_CHECK_EQUAL(expected_sorted_indices, detected); + } + + + // Check outgoing lines + std::vector seg_ids; + bglt::get_left_turns(sorted, p, seg_ids); + { + std::ostringstream out; + out << std::boolalpha; + for (std::vector::const_iterator it = + seg_ids.begin(); it != seg_ids.end(); ++it) + { + out + << " " << it->from.segment_index + << "->" << it->to.segment_index + ; + } + std::string detected = boost::trim_copy(out.str()); + BOOST_CHECK_EQUAL(expected_left_indices, detected); + } + +#if defined(TEST_WITH_SVG) + { + std::ostringstream filename; + filename << "get_left_turns_" << caseid + << "_" << string_from_type::type>::name() + << ".svg"; + + std::ofstream svg(filename.str().c_str()); + + bg::svg_mapper mapper(svg, 500, 500); + mapper.add(p); + for (typename std::vector >::const_iterator it = + angles.begin(); it != angles.end(); ++it) + { + // Add a point further then it->to_point, just for the mapping + for (int i = 0; i < 2; i++) + { + mapper.add(further_than(it->points[i], p, 12, 10)); + } + } + + int color_index = 0; + typedef bg::model::referring_segment segment_type; + for (typename std::vector >::const_iterator it = + angles.begin(); it != angles.end(); ++it, color_index++) + { + for (int i = 0; i < 2; i++) + { + std::string style = "opacity:0.5;stroke-width:1;stroke:rgb(0,0,0);fill:" + get_color(color_index); + + bool const incoming = i == 0; + Point const& pf = incoming ? it->points[i] : p; + Point const& pt = incoming ? p : it->points[i]; + vector_type v = pt; + bg::subtract_point(v, pf); + + bg::divide_value(v, 10.0); + + // Generate perpendicular vector to right-side + vector_type perpendicular; + bg::set<0>(perpendicular, bg::get<1>(v)); + bg::set<1>(perpendicular, -bg::get<0>(v)); + + bg::model::ring ring; + ring.push_back(pf); + ring.push_back(pt); + + // Extra point at 9/10 + { + Point pe = pt; + bg::add_point(pe, perpendicular); + ring.push_back(pe); + } + { + Point pe = pf; + bg::add_point(pe, perpendicular); + ring.push_back(pe); + } + ring.push_back(pf); + + mapper.map(ring, style); + + segment_type s(pf, pt); + mapper.map(s, "opacity:0.9;stroke-width:4;stroke:rgb(0,0,0);"); + } + } + + // Output angles for left-turns + for (std::vector::const_iterator ltit = + seg_ids.begin(); ltit != seg_ids.end(); ++ltit) + { + for (typename std::vector >::const_iterator sit = + sorted.begin(); sit != sorted.end(); ++sit, color_index++) + { + Point pf, pt; + int factor = 0; + if (sit->seg_id == ltit->from && sit->incoming) + { + pf = sit->point; + pt = p; + factor = -1; // left side + } + else if (sit->seg_id == ltit->to && ! sit->incoming) + { + pf = p; + pt = sit->point; + factor = -1; // left side + } + if (factor != 0) + { + vector_type v = pt; + bg::subtract_point(v, pf); + bg::divide_value(v, 10.0); + + // Generate perpendicular vector to right-side + vector_type perpendicular; + bg::set<0>(perpendicular, factor * bg::get<1>(v)); + bg::set<1>(perpendicular, -factor * bg::get<0>(v)); + + bg::add_point(pf, v); + bg::subtract_point(pt, v); + + bg::add_point(pf, perpendicular); + bg::add_point(pt, perpendicular); + + segment_type s(pf, pt); + mapper.map(s, "opacity:0.9;stroke-width:4;stroke:rgb(255,0,0);"); + } + } + } + + // Output texts with info about sorted/blocked + int index = 0; + for (typename std::vector >::const_iterator it = + sorted.begin(); it != sorted.end(); ++it, ++index) + { + std::ostringstream out; + out << std::boolalpha; + out << " seg:" << it->seg_id.segment_index + << " " << (it->incoming ? "in" : "out") + << " idx:" << index + << (it->blocked ? " blocked" : "") + ; + mapper.text(further_than(it->point, p, 11, 10), out.str(), "fill:rgb(0,0,0);font-family='Verdana'"); + } + + mapper.map(p, "fill:rgb(255,0,0)"); + + } + +#endif + +} + + +template +void test_all() +{ + using bglt::turn_angle_info; + + test_one

("cross", + bg::make

(50, 50), // ip + boost::assign::list_of + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 1), bg::make

(100, 100), bg::make

(0, 0))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 2), bg::make

(100, 0), bg::make

(0, 100))) + , "1-true 2-true 1-false 2-false" + , "2->1" + ); + + test_one

("occupied", + bg::make

(50, 50), // ip + boost::assign::list_of + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 1), bg::make

(100, 100), bg::make

(0, 0))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 2), bg::make

(100, 0), bg::make

(0, 100))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 3), bg::make

(0, 30), bg::make

(100, 70))) + , "1-true 3-false 2-true 1-false 3-true 2-false" + , "" + ); + + test_one

("uu", + bg::make

(50, 50), // ip + boost::assign::list_of + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 1), bg::make

(0, 0), bg::make

(100, 0))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 2), bg::make

(100, 100), bg::make

(0, 100))) + , "2-true 1-false 1-true 2-false" + , "2->1 1->2" + ); + + test_one

("uu2", + bg::make

(50, 50), // ip + boost::assign::list_of + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 1), bg::make

(0, 0), bg::make

(100, 0))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 2), bg::make

(100, 100), bg::make

(0, 100))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 3), bg::make

(0, 50), bg::make

(100, 50))) + , "2-true 3-false 1-false 1-true 3-true 2-false" + , "2->3 3->2" + ); + + test_one

("uu3", + bg::make

(50, 50), // ip + boost::assign::list_of + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 1), bg::make

(0, 0), bg::make

(100, 0))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 2), bg::make

(100, 100), bg::make

(0, 100))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 3), bg::make

(50, 0), bg::make

(50, 100))) + , "3-false 2-true 1-false 3-true 1-true 2-false" + , "1->2" + ); + + test_one

("longer", + bg::make

(50, 50), // ip + boost::assign::list_of + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 1), bg::make

(100, 100), bg::make

(0, 0))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 2), bg::make

(100, 0), bg::make

(0, 100))) + (turn_angle_info

(bg::segment_identifier(0, -1, -1, 3), bg::make

(90, 10), bg::make

(10, 10))) + , "1-true 2-true 3-true 1-false 3-false 2-false" + , "3->1" + ); +} + +int test_main( int , char* [] ) +{ + test_all >(); + + return 0; +}