diff --git a/Trajectory_Hotspots/Test_Trajectory_Hotspots/test_trajectory.cpp b/Trajectory_Hotspots/Test_Trajectory_Hotspots/test_trajectory.cpp index 6366e6c..02831d5 100644 --- a/Trajectory_Hotspots/Test_Trajectory_Hotspots/test_trajectory.cpp +++ b/Trajectory_Hotspots/Test_Trajectory_Hotspots/test_trajectory.cpp @@ -30,6 +30,149 @@ namespace TestTrajectoryHotspots } + TEST_METHOD(get_hotspot_fixed_radius_contiguous_non_zero) + { + std::vector test_points = { + { 114.38f, 296.93f }, + { 114.47f, 296.928f }, + { 114.29,296.871 }, + { 114.13,296.835 }, + { 113.87,296.82 }, + { 113.84,296.841 }, + { 114,296.837 }, + { 113.88,296.828 }, + { 113.73,296.827 }, + { 113.7,296.818 }, + { 113.74,296.804 }, + { 113.8,296.743 }, + { 113.59,296.755 }, + { 113.51,296.787 }, + { 113.5,296.797 }, + { 113.32,296.753 }, + { 113.16,296.722 }, + { 113.04,296.726 }, + { 112.9,296.736 }, + { 112.89,296.72 }, + { 113.15,296.708 }, + { 113.24,296.695 }, + { 113.23,296.694 }, + { 112.93,296.701 }, + { 112.69,296.707 }, + { 112.47,296.691 }, + { 112.51,296.669 }, + { 112.64,296.644 }, + { 112.8,296.596 }, + { 112.43,296.561 }, + { 111.49,296.512 }, + { 110.8,296.501 }, + { 110.91,296.525 }, + { 111.32,296.534 }, + { 111.3,296.528 }, + { 111.22,296.521 }, + { 111.13,296.449 }, + { 111.24,296.382 }, + { 111.07,296.306 }, + { 110.97,296.248 }, + { 110.37,296.21 }, + { 109.91,296.174 }, + { 110.58,296.232 }, + { 110.65,296.261 }, + { 110.33,296.359 }, + { 110.12,296.398 }, + { 109.67,296.373 }, + { 109.37,296.313 }, + { 109.03,296.236 }, + { 109.01,296.147 }, + { 110.54,296.124 }, + { 111.39,296.238 }, + { 111.61,296.305 }, + { 112.42,296.326 }, + { 112.87,296.363 }, + { 112.92,296.404 }, + { 112.73,296.415 }, + { 113.08,296.497 }, + { 113.2,296.602 }, + { 114.31,296.922 }, + { 114.34,296.943 }, + { 114.51,296.911 }, + { 114.92,296.848 }, + { 115.26,296.81 }, + { 115.39,296.803 }, + { 115.09,296.813 }, + { 114.49,296.839 }, + { 114.52,296.849 }, + { 114.54,296.854 }, + { 114.6,296.852 }, + { 114.68,296.846 }, + { 114.9,296.844 }, + { 114.73,296.862 }, + { 114.61,296.845 }, + { 114.4,296.887 }, + { 114.7,296.898 }, + { 114.8,296.888 }, + { 115.02,296.895 }, + { 114.96,296.9 }, + { 114.95,296.894 }, + { 114.36,296.912 }, + { 114.04,296.923 }, + { 113.44,296.925 }, + { 113.11,296.919 }, + { 112.86,296.905 }, + { 112.81,296.873 }, + { 113.53,296.877 }, + { 114.93,296.838 }, + { 115.89,296.836 }, + { 116.02,296.863 }, + { 116,296.855 }, + { 115.18,296.819 }, + { 115.07,296.825 }, + { 115.17,296.822 }, + { 115.41,296.826 }, + { 115.77,296.834 }, + { 116.29,296.809 }, + { 116.35,296.79 }, + { 117.07,296.904 }, + { 117.33,296.982 }, + { 117.62,297.039 }, + { 117.92,297.064 }, + { 118.5,297.06 }, + { 118.87,297.073 }, + { 119.4,297.107 }, + { 119.35,297.117 }, + { 119.1,297.13 }, + { 118.98,297.118 }, + { 118.53,297.09 }, + { 118.45,297.056 }, + { 118.43,297.051 }, + { 117.71,297.075 }, + { 117.51,297.047 }, + { 117.39,296.976 }, + { 116.78,296.884 }, + { 115.86,296.824 }, + { 116.31,296.853 }, + { 116.13,296.842 }, + { 115.82,296.786 }, + { 115.25,296.793 }, + { 115.65,296.73 }, + { 115.56,296.714 }, + { 115.52,296.754 }, + { 115.54,296.762 }, + { 115.6,296.745 }, + { 115.97,296.737 }, + { 116.36,296.796 }, + { 116.17,296.776 }, + { 115.66,296.768 }, + { 115.61,296.757 }, + { 115.59,296.758 }, + { 115.29,296.771 } }; + + Trajectory trajectory(test_points); + + AABB aabb = trajectory.get_hotspot_fixed_radius_contiguous(5.f); + + Assert::IsTrue(aabb.max_size() > 0.f); + } + //2B TEST_METHOD(get_hotspot_fixed_length_contiguous_full_diagonal) { diff --git a/Trajectory_Hotspots/Trajectory_Hotspots/trajectory.cpp b/Trajectory_Hotspots/Trajectory_Hotspots/trajectory.cpp index aa4751d..b5b6a19 100644 --- a/Trajectory_Hotspots/Trajectory_Hotspots/trajectory.cpp +++ b/Trajectory_Hotspots/Trajectory_Hotspots/trajectory.cpp @@ -57,8 +57,8 @@ AABB Trajectory::get_hotspot_fixed_radius_contiguous(Float radius) const for (auto& trajectory_segment : trajectory_segments) { - x_segments.emplace_back(Vec2(trajectory_segment.start_t, trajectory_segment.start.x), Vec2(trajectory_segment.end_t, trajectory_segment.end.x)); - y_segments.emplace_back(Vec2(trajectory_segment.start_t, trajectory_segment.start.y), Vec2(trajectory_segment.end_t, trajectory_segment.end.y)); + x_segments.emplace_back(Vec2(trajectory_segment.start_t, trajectory_segment.start.x), Vec2(trajectory_segment.end_t, trajectory_segment.end.x), trajectory_segment.start_t, trajectory_segment.end_t); + y_segments.emplace_back(Vec2(trajectory_segment.start_t, trajectory_segment.start.y), Vec2(trajectory_segment.end_t, trajectory_segment.end.y), trajectory_segment.start_t, trajectory_segment.end_t); } Trapezoidal_Map trapezoidal_map_x(x_segments); @@ -75,25 +75,25 @@ AABB Trajectory::get_hotspot_fixed_radius_contiguous(Float radius) const //We slightly diverge from the paper here by tracing left and right from both the vector and vector + or - radius and taking the shortest of both. //instead of going up from the found point on the left and tracing back. - //Because it gives the same answer and we remove a number of checks. + //This gives the same answer and we remove a number of checks. //Test vertical lines - //Test left - Vec2 vert_at_radius(current_x_vert.x, current_x_vert.y - radius); - frc_test_between_lines(trapezoidal_map_x, current_x_vert, vert_at_radius, false, segment_tree, radius, longest_valid_subtrajectory, optimal_hotspot); + //Test left, current vert is at top + Vec2 vert_at_radius(current_x_vert.x, current_x_vert.y - radius); //TODO: Remove, can calc in function.. + frc_test_between_lines(trapezoidal_map_x, current_x_vert, vert_at_radius, true, segment_tree, radius, longest_valid_subtrajectory, optimal_hotspot); - //Test right + //Test right, current vert is at bottom vert_at_radius = Vec2(current_x_vert.x, current_x_vert.y + radius); - frc_test_between_lines(trapezoidal_map_x, current_x_vert, vert_at_radius, true, segment_tree, radius, longest_valid_subtrajectory, optimal_hotspot); + frc_test_between_lines(trapezoidal_map_x, current_x_vert, vert_at_radius, false, segment_tree, radius, longest_valid_subtrajectory, optimal_hotspot); //Test horizontal lines - //Test below + //Test below, current vert is at top vert_at_radius = Vec2(current_y_vert.x, current_y_vert.y - radius); - frc_test_between_lines(trapezoidal_map_y, current_y_vert, vert_at_radius, false, segment_tree, radius, longest_valid_subtrajectory, optimal_hotspot); + frc_test_between_lines(trapezoidal_map_y, current_y_vert, vert_at_radius, true, segment_tree, radius, longest_valid_subtrajectory, optimal_hotspot); - //Test above + //Test above, current vert is at bottom vert_at_radius = Vec2(current_y_vert.x, current_y_vert.y + radius); - frc_test_between_lines(trapezoidal_map_y, current_y_vert, vert_at_radius, true, segment_tree, radius, longest_valid_subtrajectory, optimal_hotspot); + frc_test_between_lines(trapezoidal_map_y, current_y_vert, vert_at_radius, false, segment_tree, radius, longest_valid_subtrajectory, optimal_hotspot); } //TODO: Don't forget last point? Or can we skip? @@ -107,7 +107,8 @@ void Trajectory::frc_test_between_lines(Trapezoidal_Map& trapezoidal_map, Vec2& Float subtrajectory_start; Float subtrajectory_end; - frc_get_subtrajectory_start_and_end(trapezoidal_map, current_vert, vert_at_radius, above, subtrajectory_start, subtrajectory_end); + frc_get_subtrajectory_within_boundary(trapezoidal_map, current_vert, vert_at_radius, above, subtrajectory_start, subtrajectory_end); + if ((subtrajectory_end - subtrajectory_start) > longest_valid_subtrajectory) { @@ -121,35 +122,56 @@ void Trajectory::frc_test_between_lines(Trapezoidal_Map& trapezoidal_map, Vec2& } } -void Trajectory::frc_get_subtrajectory_start_and_end(const Trapezoidal_Map& trapezoidal_map, const Vec2& current_vert, const Vec2& vert_at_radius, const bool above_point, Float& subtrajectory_start, Float& subtrajectory_end) const +void Trajectory::frc_get_subtrajectory_within_boundary(const Trapezoidal_Map& trapezoidal_map, const Vec2& current_vert, const Vec2& vert_at_radius, const bool above_point, Float& subtrajectory_start, Float& subtrajectory_end) const { - //TODO: nullptr on left/right is everything before? -> Never nullptr? Check for infinity points? -> Add is_at_infinite_edge function to trapezoidal_map? + //Query the start and end of the subtrajectory from the vertex and at the point one radius away. + Float start_at_vert; + Float end_at_vert; + + Float start_at_radius; + Float end_at_radius; + + frc_get_subtrajectory_start_and_end(trapezoidal_map, current_vert, above_point, start_at_vert, end_at_vert); + frc_get_subtrajectory_start_and_end(trapezoidal_map, vert_at_radius, !above_point, start_at_radius, end_at_radius); + + //Keep the shortest start and end, the trajectory leaves the boundary at that point so the subtrajectory towards the longer intersection will leave the boundary. + subtrajectory_start = (start_at_vert < start_at_radius) ? start_at_vert : start_at_radius; + subtrajectory_end = (end_at_vert < end_at_radius) ? end_at_vert : end_at_radius; +} +void Trajectory::frc_get_subtrajectory_start_and_end(const Trapezoidal_Map& trapezoidal_map, const Vec2& query_vert, const bool above_point, Float& subtrajectory_start, Float& subtrajectory_end) const +{ const Segment* left_segment = nullptr; const Segment* right_segment = nullptr; - trapezoidal_map.trace_left_right(current_vert, !above_point, left_segment, right_segment); + trapezoidal_map.trace_left_right(query_vert, above_point, left_segment, right_segment); if (left_segment != nullptr && right_segment != nullptr) { - Float start_t = left_segment->get_time_at_y(current_vert.y); - Float end_t = right_segment->get_time_at_y(current_vert.y); - - - //trace at radius - const Segment* left_segment_plus = nullptr; - const Segment* right_segment_plus = nullptr; - - trapezoidal_map.trace_left_right(vert_at_radius, above_point, left_segment_plus, right_segment_plus); - - if (left_segment_plus != nullptr && right_segment_plus != nullptr) + //If the left_segment is at infinity there is no trajectory on the left, set start_t to the start of the trajectory + if (left_segment->start.x.is_inf()) + { + subtrajectory_start = this->trajectory_start; + } + else { - Float start_t_plus = left_segment->get_time_at_y(current_vert.y); - Float end_t_plus = right_segment->get_time_at_y(current_vert.y); + subtrajectory_start = left_segment->get_time_at_y(query_vert.y); + } - subtrajectory_start = (start_t_plus < start_t) ? start_t_plus : start_t; - subtrajectory_end = (end_t_plus < end_t) ? end_t_plus : end_t; + //If the right_segment is at infinity there is no trajectory on the right, set end_t to the end of the trajectory + if (right_segment->start.x.is_inf()) + { + subtrajectory_end = this->trajectory_end; + } + else + { + subtrajectory_end = right_segment->get_time_at_y(query_vert.y); } } + else + { + //Neither segment should ever be nullptr + assert(false); + } } //Find the smallest hotspot that contains a subtrajectory with at least the given length inside of it diff --git a/Trajectory_Hotspots/Trajectory_Hotspots/trajectory.h b/Trajectory_Hotspots/Trajectory_Hotspots/trajectory.h index 89058d8..6f8df18 100644 --- a/Trajectory_Hotspots/Trajectory_Hotspots/trajectory.h +++ b/Trajectory_Hotspots/Trajectory_Hotspots/trajectory.h @@ -5,7 +5,7 @@ class Trajectory { public: - Trajectory(){} + Trajectory() {} Trajectory(const std::vector& ordered_segments); Trajectory(const std::vector& ordered_points); @@ -28,9 +28,9 @@ class Trajectory //Helper functions for fixed_radius_contiguous - void frc_test_between_lines(Trapezoidal_Map & trapezoidal_map, Vec2 & current_vert, Vec2& vert_at_radius, bool above, Segment_Search_Tree& segment_tree, Float& radius, Float& longest_valid_subtrajectory, AABB& optimal_hotspot) const; - void frc_get_subtrajectory_start_and_end(const Trapezoidal_Map& trapezoidal_map, const Vec2& current_vert, const Vec2& vert_at_radius, const bool above_point, Float& subtrajectory_start, Float& subtrajectory_end) const; - + void frc_test_between_lines(Trapezoidal_Map& trapezoidal_map, Vec2& current_vert, Vec2& vert_at_radius, bool above, Segment_Search_Tree& segment_tree, Float& radius, Float& longest_valid_subtrajectory, AABB& optimal_hotspot) const; + void frc_get_subtrajectory_within_boundary(const Trapezoidal_Map& trapezoidal_map, const Vec2& current_vert, const Vec2& vert_at_radius, const bool above_point, Float& subtrajectory_start, Float& subtrajectory_end) const; + void frc_get_subtrajectory_start_and_end(const Trapezoidal_Map& trapezoidal_map, const Vec2& query_vert, const bool above_point, Float& subtrajectory_start, Float& subtrajectory_end) const; //Helper functions for fixed_length_contiguous bool flc_breakpoint_III_x(const Float length, const Segment& start_segment, const Segment& end_segment, const Float vertical_line_x, const AABB& uv_bounding_box, AABB& potential_hotspot) const; diff --git a/Trajectory_Hotspots/Trajectory_Hotspots/trapezoidal_map.h b/Trajectory_Hotspots/Trajectory_Hotspots/trapezoidal_map.h index d82423a..28c0063 100644 --- a/Trajectory_Hotspots/Trajectory_Hotspots/trapezoidal_map.h +++ b/Trajectory_Hotspots/Trajectory_Hotspots/trapezoidal_map.h @@ -51,6 +51,13 @@ class Trapezoidal_Leaf_Node : public Trapezoidal_Node void replace_bottom_neighbour(Trapezoidal_Leaf_Node* old_bottom_neighbour, Trapezoidal_Leaf_Node* new_bottom_neighbour); void replace_top_neighbour(Trapezoidal_Leaf_Node* old_top_neighbour, Trapezoidal_Leaf_Node* new_top_neighbour); + /// + /// Trace a horizontal ray to the left and right finding the first segments that intersect it to the left and right of the query point. + /// + /// The queried point. + /// When the query point lies on an endpoint within the trapezoidal map, we trace either above or below based on this parameter. This prevents unwanted self intersections. + /// The first segment to the left of the queried point. + /// The first segment to the right of the queried point. void trace_left_right(const Vec2& point, const bool prefer_top, const Segment*& left_segment, const Segment*& right_segment) const; Trapezoidal_Leaf_Node* bottom_left;