From 1e5e203d5b77772e18c0ceafdf92a8b3cf282416 Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Thu, 9 Feb 2023 14:24:12 -0300 Subject: [PATCH] Precomputes linestring's segments intervals (#51) Signed-off-by: Franco Cipollone --- include/maliput_sparse/geometry/line_string.h | 87 +++++++-- .../geometry/utility/geometry.h | 12 +- src/geometry/CMakeLists.txt | 4 +- src/geometry/utility/geometry.cc | 165 ++++++++---------- test/base/lane_test.cc | 8 + test/geometry/line_string_test.cc | 103 +++++++++++ test/geometry/utility/geometry_test.cc | 43 ++--- 7 files changed, 286 insertions(+), 136 deletions(-) diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h index 85ec037..16ca699 100644 --- a/include/maliput_sparse/geometry/line_string.h +++ b/include/maliput_sparse/geometry/line_string.h @@ -32,8 +32,10 @@ #include #include #include +#include #include +#include #include #include @@ -74,6 +76,52 @@ class LineString final { using iterator = typename std::vector::iterator; using const_iterator = typename std::vector::const_iterator; + /// A segment of a LineString. + /// A segment is defined by a: + /// - start index: index of the first coordinate of the segment. + /// - end index: index of the last coordinate of the segment, it is expected to be `start index + 1` as `LineString`'s + /// constructor + /// builds the segments from consecutive coordinates. + struct Segment { + /// Defines an interval in the @f$ p @f$ value of the parametrized LineString. + /// The Less than operator is defined to allow the use of this struct as a key in a collection like std::map. + struct Interval { + /// Creates a Interval. + /// @param min_in Is the minimum value of the interval. + /// @param max_in Is the maximum value of the interval. + /// @throw maliput::common::assertion_error When `min_in` is greater than `max_in`. + Interval(double min_in, double max_in) : min(min_in), max(max_in) { MALIPUT_THROW_UNLESS(min_in <= max_in); } + + /// Creates a Interval where + /// the minimum value is equal to the maximum value. + /// @param min_max Is the minimum and maximum value of the interval. + Interval(double min_max) : min(min_max), max(min_max) {} + + /// Less than operator. + bool operator<(const Interval& rhs) const { + if (min < rhs.min) { + return max <= rhs.max ? true : false; + } else { + return false; + } + } + + // Min p value. + double min{}; + // Max p value. + double max{}; + }; + + /// Start index of the segment. + std::size_t idx_start; + /// End index of the segment. + std::size_t idx_end; + /// Interval of the segment. + Segment::Interval p_interval; + }; + + using Segments = std::map; + /// Constructs a LineString from a std::vector. /// /// This function calls LineString(coordinates.begin, coordinates.end) @@ -101,8 +149,31 @@ class LineString final { /// @throws maliput::common::assertion_error When there are less than two points. template LineString(Iterator begin, Iterator end) : coordinates_(begin, end) { + // Remove consecutive points that are numerically the same. + // Duplicated points creates zero length segments, which leads to a wrong lookup when querying the segment map. + std::vector remove_idx; + for (std::size_t idx{}; idx < coordinates_.size() - 1; ++idx) { + const double segment_length = DistanceFunction()(coordinates_[idx], coordinates_[idx + 1]); + if (segment_length <= std::numeric_limits::epsilon()) { + maliput::log()->warn("LineString: consecutive points are numerically the same, removing duplicated point: {}", + coordinates_[idx + 1]); + remove_idx.push_back(idx + 1); + } + } + // Remove the duplicated points. + for (auto it = remove_idx.rbegin(); it != remove_idx.rend(); ++it) { + coordinates_.erase(coordinates_.begin() + *it); + } MALIPUT_THROW_UNLESS(coordinates_.size() > 1); - length_ = ComputeLength(); + // Fill up the segments collection + double p = 0; + for (std::size_t idx{}; idx < coordinates_.size() - 1; ++idx) { + const double segment_length = DistanceFunction()(coordinates_[idx], coordinates_[idx + 1]); + const typename Segment::Interval interval{p, p + segment_length}; + segments_.emplace(interval, Segment{idx, idx + 1, interval}); + p += segment_length; + } + length_ = p; } /// @return The first point in the LineString. @@ -121,6 +192,10 @@ class LineString final { /// @return The accumulated length between consecutive points in this LineString by means of DistanceFunction. double length() const { return length_; } + /// Return the segments of this LineString. + /// @return A vector of segments. + const Segments& segments() const { return segments_; } + /// @returns begin iterator of the underlying collection. iterator begin() { return coordinates_.begin(); } /// @returns begin const iterator of the underlying collection. @@ -139,16 +214,8 @@ class LineString final { } private: - // @return The accumulated Length of this LineString. - const double ComputeLength() const { - double accumulated_{0.}; - for (size_t i = 0; i < size() - 1; ++i) { - accumulated_ += DistanceFunction()(at(i), at(i + 1)); - } - return accumulated_; - } - std::vector coordinates_{}; + Segments segments_{}; double length_{}; }; diff --git a/include/maliput_sparse/geometry/utility/geometry.h b/include/maliput_sparse/geometry/utility/geometry.h index 552889e..d7548de 100644 --- a/include/maliput_sparse/geometry/utility/geometry.h +++ b/include/maliput_sparse/geometry/utility/geometry.h @@ -38,18 +38,13 @@ namespace geometry { namespace utility { /// Holds the result of #GetBoundPointsAtP method. -/// @tparam CoordinateT The coordinate type. -template struct BoundPointsResult { - typename LineString::const_iterator first; - typename LineString::const_iterator second; + std::size_t idx_start; + std::size_t idx_end; // Length up to first. double length; }; -using BoundPointsResult3d = BoundPointsResult; -using BoundPointsResult2d = BoundPointsResult; - /// Holds the result of #GetClosestPoint method. /// @tparam CoordinateT The coordinate type. template @@ -101,8 +96,7 @@ double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance); /// @param tolerance tolerance. /// @throws maliput::common::assertion_error When `p ∉ [0., line_string.length()]`. template -BoundPointsResult GetBoundPointsAtP(const LineString& line_string, double p, - double tolerance); +BoundPointsResult GetBoundPointsAtP(const LineString& line_string, double p, double tolerance); /// Returns the heading of a @p line_string for a given @p p . /// @param line_string LineString to be computed the heading from. diff --git a/src/geometry/CMakeLists.txt b/src/geometry/CMakeLists.txt index 107e841..c024a89 100644 --- a/src/geometry/CMakeLists.txt +++ b/src/geometry/CMakeLists.txt @@ -24,8 +24,8 @@ target_include_directories(geometry target_link_libraries(geometry PUBLIC - maliput::common - maliput::math + maliput::common + maliput::math ) ############################################################################## diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc index 04c761e..08c6ba6 100644 --- a/src/geometry/utility/geometry.cc +++ b/src/geometry/utility/geometry.cc @@ -84,14 +84,6 @@ Vector2 To2D(const Vector3& vector) { return {vector.x(), vector.y()}; } Segment2d To2D(const Segment3d& segment) { return {To2D(segment.first), To2D(segment.second)}; } -LineString2d To2D(const LineString3d& line_string) { - std::vector points; - for (const auto& point : line_string) { - points.push_back(To2D(point)); - } - return LineString2d{points}; -} - // Determines whether two line segments intersects. // // Based on https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line_segment @@ -293,60 +285,45 @@ CoordinateT InterpolatedPointAtP(const LineString& line_string, dou // https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/include/lanelet2_core/geometry/impl/LineString.h#L618 if (p < 0) return line_string.first(); if (p >= line_string.length()) return line_string.last(); - const auto line_string_points_length = GetBoundPointsAtP(line_string, p, tolerance); - const double partial_length{(*line_string_points_length.first - *line_string_points_length.second).norm()}; - const double remaining_distance = p - line_string_points_length.length; - if (remaining_distance < kEpsilon) { - return *line_string_points_length.first; - } - return *line_string_points_length.first + - remaining_distance / partial_length * (*line_string_points_length.second - *line_string_points_length.first); + const auto bound_points = GetBoundPointsAtP(line_string, p, tolerance); + const CoordinateT& start = line_string[bound_points.idx_start]; + const CoordinateT& end = line_string[bound_points.idx_end]; + const CoordinateT d_segment{end - start}; + const double remaining_distance = p - bound_points.length; + return remaining_distance < kEpsilon ? start : start + d_segment.normalized() * remaining_distance; } double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance) { const BoundPointsResult bound_points = GetBoundPointsAtP(line_string, p, tolerance); - const double dist{(To2D(*bound_points.second) - To2D(*bound_points.first)).norm()}; - const double delta_z{bound_points.second->z() - bound_points.first->z()}; - MALIPUT_THROW_UNLESS(*bound_points.second != *bound_points.first); + const maliput::math::Vector3& start = line_string[bound_points.idx_start]; + const maliput::math::Vector3& end = line_string[bound_points.idx_end]; + const double delta_z{end.z() - start.z()}; + const double dist{(To2D(end) - To2D(start)).norm()}; + MALIPUT_THROW_UNLESS(start != end); return delta_z / dist; } template -BoundPointsResult GetBoundPointsAtP(const LineString& line_string, double p, - double tolerance) { - maliput::common::RangeValidator::GetAbsoluteEpsilonValidator(0., line_string.length(), tolerance, kEpsilon)(p); - BoundPointsResult result; - double current_cumulative_length = 0.0; - for (auto first = line_string.begin(), second = std::next(line_string.begin()); second != line_string.end(); - ++first, ++second) { - const auto p1 = *first; - const auto p2 = *second; - const double current_length = (p1 - p2).norm(); - if (current_cumulative_length + current_length >= p) { - return {first, second, current_cumulative_length}; - } - current_cumulative_length += current_length; - } - return {line_string.end() - 1, line_string.end() - 2, line_string.length()}; +BoundPointsResult GetBoundPointsAtP(const LineString& line_string, double p, double tolerance) { + p = maliput::common::RangeValidator::GetAbsoluteEpsilonValidator(0., line_string.length(), tolerance, kEpsilon)(p); + const auto segment_itr = line_string.segments().find({p}); + return {segment_itr->second.idx_start, segment_itr->second.idx_end, segment_itr->second.p_interval.min}; } double Get2DHeadingAtP(const LineString3d& line_string, double p, double tolerance) { - const auto line_string_points_length = GetBoundPointsAtP(line_string, p, tolerance); - const Vector3 heading_vector{*line_string_points_length.second - *line_string_points_length.first}; + const auto bound_points = GetBoundPointsAtP(line_string, p, tolerance); + const Vector3 heading_vector{line_string[bound_points.idx_end] - line_string[bound_points.idx_start]}; return std::atan2(heading_vector.y(), heading_vector.x()); } Vector2 Get2DTangentAtP(const LineString3d& line_string, double p, double tolerance) { - // const double heading = Get2DHeadingAtP(line_string, p); - // return {std::cos(heading), std::sin(heading)}; - const auto line_string_points_length = GetBoundPointsAtP(line_string, p, tolerance); - const Vector2 d_xy{To2D(*line_string_points_length.second) - To2D(*line_string_points_length.first)}; - return (d_xy / (To2D(line_string).length())).normalized(); + const double heading = Get2DHeadingAtP(line_string, p, tolerance); + return {std::cos(heading), std::sin(heading)}; } Vector3 GetTangentAtP(const LineString3d& line_string, double p, double tolerance) { - const auto line_string_points_length = GetBoundPointsAtP(line_string, p, tolerance); - const Vector3 d_xyz{*line_string_points_length.second - *line_string_points_length.first}; + const auto bound_points = GetBoundPointsAtP(line_string, p, tolerance); + const Vector3 d_xyz{line_string[bound_points.idx_end] - line_string[bound_points.idx_start]}; return (d_xyz / (line_string.length())).normalized(); } @@ -354,62 +331,74 @@ template ClosestPointResult GetClosestPointToSegment(const std::pair& segment, const CoordinateT& coordinate, double tolerance) { const CoordinateT d_segment{segment.second - segment.first}; + const CoordinateT d_segment_normalized{d_segment.normalized()}; const CoordinateT d_coordinate_to_first{coordinate - segment.first}; - const double unsaturated_p = d_coordinate_to_first.dot(d_segment.normalized()); + const double unsaturated_p = d_coordinate_to_first.dot(d_segment_normalized); const double p = std::clamp(unsaturated_p, 0., d_segment.norm()); - const CoordinateT point = InterpolatedPointAtP(LineString{segment.first, segment.second}, p, tolerance); + + // point at p + const CoordinateT point = p * d_segment_normalized + segment.first; const double distance = (coordinate - point).norm(); return {p, point, distance}; } ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const maliput::math::Vector3& xyz, double tolerance) { - ClosestPointResult3d result; - result.distance = std::numeric_limits::max(); - double length{}; - for (auto first = line_string.begin(), second = std::next(line_string.begin()); second != line_string.end(); - ++first, ++second) { - // If points are under numeric tolerance, skip segment. - if ((*first - *second).norm() < kEpsilon) continue; - const auto closest_point_res = GetClosestPointToSegment(Segment3d{*first, *second}, xyz, tolerance); - if (closest_point_res.distance < result.distance) { - result = closest_point_res; - result.p += length; + std::optional closest_segment{std::nullopt}; + ClosestPointResult3d segment_closest_point_result; + segment_closest_point_result.distance = std::numeric_limits::max(); + + const auto& segments = line_string.segments(); + std::for_each(segments.begin(), segments.end(), [&](const auto& segment) { + const auto& start = line_string[segment.second.idx_start]; + const auto& end = line_string[segment.second.idx_end]; + const auto current_closest_point_res = GetClosestPointToSegment(Segment3d{start, end}, xyz, tolerance); + if (current_closest_point_res.distance < segment_closest_point_result.distance) { + segment_closest_point_result = current_closest_point_res; + closest_segment = {segment.second.idx_start, segment.second.idx_end, segment.second.p_interval}; } - length += (*second - *first).norm(); //> Adds segment length - } - return result; + }); + + return {segment_closest_point_result.p + closest_segment->p_interval.min, segment_closest_point_result.point, + segment_closest_point_result.distance}; } ClosestPointResult3d GetClosestPointUsing2dProjection(const LineString3d& line_string, const maliput::math::Vector3& xyz, double tolerance) { - ClosestPointResult3d result; - result.distance = std::numeric_limits::max(); - double length{}; - for (auto first = line_string.begin(), second = std::next(line_string.begin()); second != line_string.end(); - ++first, ++second) { - // If points are under numeric tolerance, skip segment. - if ((*first - *second).norm() < kEpsilon) continue; - const maliput::math::Vector2 first_2d = To2D(*first); - const maliput::math::Vector2 second_2d = To2D(*second); - const maliput::math::Vector2 xy = To2D(xyz); - const Segment2d segment_2d{first_2d, second_2d}; - const auto closest_point_res = GetClosestPointToSegment(segment_2d, xy, tolerance); - if (closest_point_res.distance < result.distance) { - const LineString3d segment_linestring_3d{*first, *second}; - const double scale_p = segment_linestring_3d.length() / (segment_2d.first - segment_2d.second).norm(); - const maliput::math::Vector3 closest_point_3d{ - closest_point_res.point.x(), closest_point_res.point.y(), - InterpolatedPointAtP(segment_linestring_3d, closest_point_res.p * scale_p, tolerance).z()}; - - result.distance = (xyz - closest_point_3d).norm(); - result.point = closest_point_3d; - result.p = length + closest_point_res.p * scale_p; + const maliput::math::Vector2 xy = To2D(xyz); + + // Find the closest segment in 2D + std::optional closest_segment{std::nullopt}; + ClosestPointResult2d segment_closest_point_result; + segment_closest_point_result.distance = std::numeric_limits::max(); + + const auto& segments = line_string.segments(); + std::for_each(segments.begin(), segments.end(), [&](const auto& segment) { + const auto& start = line_string[segment.second.idx_start]; + const auto& end = line_string[segment.second.idx_end]; + const Segment2d segment_2d{To2D(start), To2D(end)}; + const auto current_closest_point_res = GetClosestPointToSegment(segment_2d, xy, tolerance); + if (current_closest_point_res.distance < segment_closest_point_result.distance) { + segment_closest_point_result = current_closest_point_res; + closest_segment = {segment.second.idx_start, segment.second.idx_end, segment.second.p_interval}; } - length += (*second - *first).norm(); //> Adds segment length - } - return result; + }); + + // Analyze closest segment and compute the closest point in 3D. + const auto& start_point = line_string[closest_segment->idx_start]; + const auto& end_point = line_string[closest_segment->idx_end]; + const double segment_3d_length = (end_point - start_point).norm(); + const Segment2d segment_2d{To2D(start_point), To2D(end_point)}; + + const double scale_p = segment_3d_length / (segment_2d.first - segment_2d.second).norm(); + const double p_3d = segment_closest_point_result.p * scale_p; + const double z_coordinate = start_point.z() + ((end_point - start_point).normalized() * p_3d).z(); + // Compound closest point in 3d. + const maliput::math::Vector3 closest_point_3d{segment_closest_point_result.point.x(), + segment_closest_point_result.point.y(), z_coordinate}; + + return {p_3d + closest_segment->p_interval.min, closest_point_3d, (xyz - closest_point_3d).norm()}; } double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, double tolerance) { @@ -425,18 +414,12 @@ double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, double // Explicit instantiations -template BoundPointsResult3d GetBoundPointsAtP(const LineString3d&, double, double); -template BoundPointsResult2d GetBoundPointsAtP(const LineString2d&, double, double); - template maliput::math::Vector3 InterpolatedPointAtP(const LineString3d&, double, double); template maliput::math::Vector2 InterpolatedPointAtP(const LineString2d&, double, double); template ClosestPointResult3d GetClosestPointToSegment(const Segment3d&, const maliput::math::Vector3&, double); template ClosestPointResult2d GetClosestPointToSegment(const Segment2d&, const maliput::math::Vector2&, double); -template class BoundPointsResult; -template class BoundPointsResult; - template class ClosestPointResult; template class ClosestPointResult; diff --git a/test/base/lane_test.cc b/test/base/lane_test.cc index 15867ef..2bbe659 100644 --- a/test/base/lane_test.cc +++ b/test/base/lane_test.cc @@ -95,12 +95,14 @@ std::vector LaneTestCases() { { {50., 0., 0.}, {50., -2., -2.}, + {100. + (102. * std::sqrt(2.) / 2.), 0., 0.}, {100. + 102. * std::sqrt(2.), 0., 0.}, {100. + 102. * std::sqrt(2.) + 98., -1., 4.}, } /* srh */, { {50., 0., 100.}, {50., -2., 98.}, + {151., 51., 100.}, {202., 102., 100.}, {203., 200., 104.}, } /* expected_backend_pos */, @@ -109,6 +111,7 @@ std::vector LaneTestCases() { Rotation::FromRpy(0., 0., 0.), Rotation::FromRpy(0., 0., M_PI / 4.), Rotation::FromRpy(0., 0., M_PI / 2.), + Rotation::FromRpy(0., 0., M_PI / 2.), } /* expected_rotation */, 100. + 102. * std::sqrt(2.) + 98. /* expected_length */, {{ @@ -117,6 +120,11 @@ std::vector LaneTestCases() { { {50., -2., -2.} /* lane_position */, {50., -2., 98.} /* nearest_position */, 0. /* distance */ }, + { + {100. + (102. * std::sqrt(2.) / 2.), 0., 0.} /* lane_position */, + {151., 51., 100.} /* nearest_position */, + 0. /* distance */ + }, { {100. + 102. * std::sqrt(2.), 0., 0.} /* lane_position */, {202., 102., 100.} /* nearest_position */, diff --git a/test/geometry/line_string_test.cc b/test/geometry/line_string_test.cc index 2a9b07f..49b0236 100644 --- a/test/geometry/line_string_test.cc +++ b/test/geometry/line_string_test.cc @@ -54,6 +54,79 @@ struct SquaredDistanceFunction { double operator()(const VectorT& v1, const VectorT& v2) const { return std::pow((v1 - v2).norm(), 2.); } }; +class SegmentIntervalTest : public ::testing::Test {}; + +TEST_F(SegmentIntervalTest, Constructors) { + EXPECT_NO_THROW((LineString3d::Segment::Interval{0., 1.})); + EXPECT_THROW((LineString3d::Segment::Interval{1., 0.}), maliput::common::assertion_error); +} + +TEST_F(SegmentIntervalTest, ValuesUsing2ArgsConstructor) { + const LineString3d::Segment::Interval dut(0., 1.); + EXPECT_EQ(0., dut.min); + EXPECT_EQ(1., dut.max); +} + +TEST_F(SegmentIntervalTest, ValuesUsing1ArgsConstructor) { + const LineString3d::Segment::Interval dut(10.); + EXPECT_EQ(10., dut.min); + EXPECT_EQ(10., dut.max); +} + +TEST_F(SegmentIntervalTest, OperatorLessThan) { + const LineString3d::Segment::Interval dut(0., 10.); + // True because dut's min is less than the other's min. + EXPECT_TRUE(dut < LineString3d::Segment::Interval(5., 15.)); + // False because dut's min is greater than the other's max. + EXPECT_FALSE(dut < LineString3d::Segment::Interval(-5., 0.)); + // True because both p are greater than dut's. + EXPECT_TRUE(dut < LineString3d::Segment::Interval(15., 20.)); + // False because the intervals are equal. + EXPECT_FALSE(dut < LineString3d::Segment::Interval(0., 10.)); +} + +TEST_F(SegmentIntervalTest, Map) { + std::map map{ + {LineString3d::Segment::Interval(0., 10.), 0}, {LineString3d::Segment::Interval(10., 20.), 1}, + {LineString3d::Segment::Interval(20., 30.), 2}, {LineString3d::Segment::Interval(30., 40.), 3}, + {LineString3d::Segment::Interval(40., 50.), 4}, {LineString3d::Segment::Interval(50., 60.), 5}, + {LineString3d::Segment::Interval(60., 70.), 6}, {LineString3d::Segment::Interval(70., 80.), 7}, + {LineString3d::Segment::Interval(80., 90.), 8}, {LineString3d::Segment::Interval(90., 100.), 9}, + }; + EXPECT_EQ(0, map.at(LineString3d::Segment::Interval(0.))); + EXPECT_EQ(0, map.at(LineString3d::Segment::Interval(5.))); + EXPECT_EQ(1, map.at(LineString3d::Segment::Interval(10.))); + EXPECT_EQ(1, map.at(LineString3d::Segment::Interval(15.))); + EXPECT_EQ(2, map.at(LineString3d::Segment::Interval(20.))); +} + +class SegmentTest : public ::testing::Test { + public: + const std::vector points_3d_ = { + maliput::math::Vector3(0., 0., 0.), + maliput::math::Vector3(10., 0., 0.), + maliput::math::Vector3(20., 0., 0.), + }; + LineString3d::Segment::Interval interval_1{0., 10.}; + LineString3d::Segment::Interval interval_2{10., 20.}; +}; + +TEST_F(SegmentTest, Constructor) { EXPECT_NO_THROW((LineString3d::Segment{0, 1, interval_1})); } + +TEST_F(SegmentTest, Values) { + const LineString3d::Segment dut_1{0, 1, interval_1}; + EXPECT_EQ(static_cast(0), dut_1.idx_start); + EXPECT_EQ(static_cast(1), dut_1.idx_end); + EXPECT_EQ(interval_1.min, dut_1.p_interval.min); + EXPECT_EQ(interval_1.max, dut_1.p_interval.max); + + const LineString3d::Segment dut_2{1, 2, interval_2}; + EXPECT_EQ(static_cast(1), dut_2.idx_start); + EXPECT_EQ(static_cast(2), dut_2.idx_end); + EXPECT_EQ(interval_2.min, dut_2.p_interval.min); + EXPECT_EQ(interval_2.max, dut_2.p_interval.max); +} + class LineString3dTest : public ::testing::Test { public: const Vector3 p1{Vector3::UnitX()}; @@ -85,6 +158,36 @@ TEST_F(LineString3dTest, Api) { EXPECT_NEAR(2. * std::sqrt(2.), dut.length(), kTolerance); } +TEST_F(LineString3dTest, SameConsecutivePoints) { + const LineString3d dut(std::vector{p1, p2, p3, p3, p2, p1, p1, p2}); + const std::vector expected_points{p1, p2, p3, p2, p1, p2}; + ASSERT_EQ(expected_points.size(), dut.size()); + for (size_t i = 0; i < dut.size() - 1; ++i) { + EXPECT_EQ(expected_points.at(i), dut.at(i)); + } +} + +TEST_F(LineString3dTest, Segments) { + const LineString3d dut(std::vector{p1, p2, p3}); + const auto segments = dut.segments(); + ASSERT_EQ(static_cast(2), segments.size()); + + const double p_in_p1_p2 = (p1 - p2).norm() / 2.; + const double p_in_p2_p3 = (p1 - p2).norm() + (p2 - p3).norm() / 2.; + + const auto segment_p1_p2 = segments.at(LineString3d::Segment::Interval(p_in_p1_p2)); + EXPECT_TRUE(p1 == dut[segment_p1_p2.idx_start]); + EXPECT_TRUE(p2 == dut[segment_p1_p2.idx_end]); + EXPECT_NEAR(0., segment_p1_p2.p_interval.min, kTolerance); + EXPECT_NEAR((p1 - p2).norm(), segment_p1_p2.p_interval.max, kTolerance); + + const auto segment_p2_p3 = segments.at(LineString3d::Segment::Interval(p_in_p2_p3)); + EXPECT_TRUE(p2 == dut[segment_p2_p3.idx_start]); + EXPECT_TRUE(p3 == dut[segment_p2_p3.idx_end]); + EXPECT_NEAR((p1 - p2).norm(), segment_p2_p3.p_interval.min, kTolerance); + EXPECT_NEAR((p1 - p2).norm() + (p2 - p3).norm(), segment_p2_p3.p_interval.max, kTolerance); +} + TEST_F(LineString3dTest, LengthInjectedDistanceFunction) { const LineString> dut(std::vector{p1, p2, p3}); EXPECT_NEAR(4., dut.length(), 1e-14); diff --git a/test/geometry/utility/geometry_test.cc b/test/geometry/utility/geometry_test.cc index 2ce3d36..0c16d10 100644 --- a/test/geometry/utility/geometry_test.cc +++ b/test/geometry/utility/geometry_test.cc @@ -242,34 +242,34 @@ class GetBoundPointsAtPTest : public ::testing::Test { TEST_F(GetBoundPointsAtPTest, Test) { { const double p{0.}; - const BoundPointsResult3d expected_result{line_string.begin(), line_string.begin() + 1, 0.}; - const BoundPointsResult3d dut = GetBoundPointsAtP(line_string, p, kTolerance); - EXPECT_EQ(expected_result.first, dut.first); - EXPECT_EQ(expected_result.second, dut.second); + const BoundPointsResult expected_result{0, 1, 0.}; + const BoundPointsResult dut = GetBoundPointsAtP(line_string, p, kTolerance); + EXPECT_EQ(expected_result.idx_start, dut.idx_start); + EXPECT_EQ(expected_result.idx_end, dut.idx_end); EXPECT_EQ(expected_result.length, dut.length); } { const double p{7.5}; - const BoundPointsResult3d expected_result{line_string.begin() + 1, line_string.begin() + 2, 7.}; - const BoundPointsResult3d dut = GetBoundPointsAtP(line_string, p, kTolerance); - EXPECT_EQ(expected_result.first, dut.first); - EXPECT_EQ(expected_result.second, dut.second); + const BoundPointsResult expected_result{1, 2, 7.}; + const BoundPointsResult dut = GetBoundPointsAtP(line_string, p, kTolerance); + EXPECT_EQ(expected_result.idx_start, dut.idx_start); + EXPECT_EQ(expected_result.idx_end, dut.idx_end); EXPECT_EQ(expected_result.length, dut.length); } { const double p{12.5}; - const BoundPointsResult3d expected_result{line_string.begin() + 2, line_string.begin() + 3, 12.}; - const BoundPointsResult3d dut = GetBoundPointsAtP(line_string, p, kTolerance); - EXPECT_EQ(expected_result.first, dut.first); - EXPECT_EQ(expected_result.second, dut.second); + const BoundPointsResult expected_result{2, 3, 12.}; + const BoundPointsResult dut = GetBoundPointsAtP(line_string, p, kTolerance); + EXPECT_EQ(expected_result.idx_start, dut.idx_start); + EXPECT_EQ(expected_result.idx_end, dut.idx_end); EXPECT_EQ(expected_result.length, dut.length); } { const double p{19.5}; - const BoundPointsResult3d expected_result{line_string.begin() + 3, line_string.begin() + 4, 19.}; - const BoundPointsResult3d dut = GetBoundPointsAtP(line_string, p, kTolerance); - EXPECT_EQ(expected_result.first, dut.first); - EXPECT_EQ(expected_result.second, dut.second); + const BoundPointsResult expected_result{3, 4, 19.}; + const BoundPointsResult dut = GetBoundPointsAtP(line_string, p, kTolerance); + EXPECT_EQ(expected_result.idx_start, dut.idx_start); + EXPECT_EQ(expected_result.idx_end, dut.idx_end); EXPECT_EQ(expected_result.length, dut.length); } } @@ -291,19 +291,19 @@ std::vector SlopeTestCases() { { // LineString with different z values along y axis. LineString3d{{0., 0., 0.}, {5., 0., 5.}, {10., 0., 5.}, {15., 0., 10.}} /* line string*/, - {0., std::sqrt(2.) * 5, std::sqrt(2.) * 5 + 5., 2 * std::sqrt(2.) * 5 + 5.} /* ps */, + {0., std::sqrt(2.) * 5. / 2., std::sqrt(2.) * 5. + 5. / 2., 2 * std::sqrt(2.) * 5. + 5.} /* ps */, {1., 1., 0., 1.} /* expected_slopes */ }, { // LineString with different z values along x axis. LineString3d{{0., 0., 0.}, {0., 5., 5.}, {0., 10., 5.}, {0., 15., 10.}} /* line string*/, - {0., std::sqrt(2.) * 5, std::sqrt(2.) * 5 + 5., 2 * std::sqrt(2.) * 5 + 5.} /* ps */, + {0., std::sqrt(2.) * 5. / 2., std::sqrt(2.) * 5. + 5. / 2., 2 * std::sqrt(2.) * 5. + 5.} /* ps */, {1., 1., 0., 1.} /* expected_slopes */ }, { // LineString with different z values. LineString3d{{0., 0., 0.}, {6., 3., 2.}, {10., 6., 2.}, {16., 9., 4.}, {10., 6., 2.}} /* line string*/, - {0., 7., 12., 19., 26.} /* ps */, + {0., 3.5, 10.5, 17., 26.} /* ps */, {2. / std::sqrt(45), 2. / std::sqrt(45), 0., 2. / std::sqrt(45), -2. / std::sqrt(45)} /* expected_slopes */ }, }; @@ -330,11 +330,6 @@ class GetSlopeAtPSpecialCasesTest : public testing::Test { static constexpr double kTolerance{1e-12}; }; -TEST_F(GetSlopeAtPSpecialCasesTest, Throw) { - const LineString3d kNoLength{{0., 0., 0.}, {0., 0., 0.}}; - EXPECT_THROW(GetSlopeAtP(kNoLength, 0., kTolerance), maliput::common::assertion_error); -} - TEST_F(GetSlopeAtPSpecialCasesTest, InfinitySlope) { const double inf{std::numeric_limits::infinity()}; const LineString3d kOnlyZ{{0., 0., 0.}, {0., 0., 100.}, {0., 0., 0.}};