Relies on GetClosestPoint for the GetClosestPointUsing2dProjection method.

include/maliput_sparse/geometry/utility/geometry.h

@@ -45,6 +45,18 @@ struct BoundPointsResult {
   double length;
 };
 
+/// Holds the result of #GetClosestPointToSegment method.
+/// @tparam CoordinateT The coordinate type.
+template <typename CoordinateT>
+struct ClosestPointToSegmentResult {
+  /// P coordinate in the line string matching the closest point.
+  double p;
+  /// Closest point.
+  CoordinateT point;
+  /// Distance between the closest point and the given point.
+  double distance;
+};
+
 /// Holds the result of #GetClosestPoint method.
 /// @tparam CoordinateT The coordinate type.
 template <typename CoordinateT>
@@ -55,10 +67,14 @@ struct ClosestPointResult {
   CoordinateT point;
   /// Distance between the closest point and the given point.
   double distance;
+  /// Linestring's segment that contains the closest point.
+  typename LineString<CoordinateT>::Segment segment;
 };
 
 using ClosestPointResult3d = ClosestPointResult<maliput::math::Vector3>;
 using ClosestPointResult2d = ClosestPointResult<maliput::math::Vector2>;
+using ClosestPointToSegmentResult3d = ClosestPointToSegmentResult<maliput::math::Vector3>;
+using ClosestPointToSegmentResult2d = ClosestPointToSegmentResult<maliput::math::Vector2>;
 
 /// Computes a 3-dimensional centerline out of the @p left and @p right line string.
 ///
@@ -129,9 +145,9 @@ maliput::math::Vector3 GetTangentAtP(const LineString3d& line_string, double p,
 /// @return A ClosestPointResult struct containing the closest point, the distance between the closest point and @p xyz
 /// and the p coordinate in the segment matching the closest point.
 template <typename CoordinateT = maliput::math::Vector3>
-ClosestPointResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
-                                                         const CoordinateT& end_segment_point,
-                                                         const CoordinateT& coordinate, double tolerance);
+ClosestPointToSegmentResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
+                                                                  const CoordinateT& end_segment_point,
+                                                                  const CoordinateT& coordinate, double tolerance);
 
 /// Gets the closest point in the @p line_string to the given @p xyz point.
 /// @param line_string LineString3d to be computed the closest point from.

src/geometry/utility/geometry.cc

@@ -328,9 +328,9 @@ Vector3 GetTangentAtP(const LineString3d& line_string, double p, double toleranc
 }
 
 template <typename CoordinateT>
-ClosestPointResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
-                                                         const CoordinateT& end_segment_point,
-                                                         const CoordinateT& coordinate, double tolerance) {
+ClosestPointToSegmentResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
+                                                                  const CoordinateT& end_segment_point,
+                                                                  const CoordinateT& coordinate, double tolerance) {
   if (start_segment_point == end_segment_point) {
     return {0., start_segment_point, (start_segment_point - coordinate).norm()};
   }
@@ -350,7 +350,7 @@ ClosestPointResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& star
 ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const maliput::math::Vector3& xyz,
                                      double tolerance) {
   std::optional<LineString3d::Segment> closest_segment{std::nullopt};
-  ClosestPointResult3d segment_closest_point_result;
+  ClosestPointToSegmentResult3d segment_closest_point_result;
   segment_closest_point_result.distance = std::numeric_limits<double>::max();
 
   const std::vector<LineString3d::Point>& line_string_points = line_string.points();
@@ -395,43 +395,35 @@ ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const mali
                                         nearest_point.p().value(), line_string_points.at(nearest_idx + 1).p().value()}};
   }
   return {segment_closest_point_result.p + closest_segment->p_interval.min, segment_closest_point_result.point,
-          segment_closest_point_result.distance};
+          segment_closest_point_result.distance, closest_segment.value()};
 }
 
 ClosestPointResult3d GetClosestPointUsing2dProjection(const LineString3d& line_string,
                                                       const maliput::math::Vector3& xyz, double tolerance) {
+  // Get the closest segment in 3d.
+  const LineString3d::Segment closest_segment_3d = GetClosestPoint(line_string, xyz, tolerance).segment;
+  const auto& start = line_string[closest_segment_3d.idx_start];
+  const auto& end = line_string[closest_segment_3d.idx_end];
+
+  // Once we have the segment let's project the point to the segment in 2d.
+  const auto start_2d = To2D(start);
+  const auto end_2d = To2D(end);
   const maliput::math::Vector2 xy = To2D(xyz);
-
-  // Find the closest segment in 2D
-  std::optional<LineString3d::Segment> closest_segment{std::nullopt};
-  ClosestPointResult2d segment_closest_point_result;
-  segment_closest_point_result.distance = std::numeric_limits<double>::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(To2D(start), To2D(end), 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};
-    }
-  });
-
-  // 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();
+  // Get the closest point in 2d.
+  const ClosestPointToSegmentResult2d closest_point_to_segment_2d =
+      GetClosestPointToSegment(start_2d, end_2d, xy, tolerance);
+
+  // Scale the p coordinate in 2d to the 3d segment length to obtain the correct p coordinate in 3d.
+  const double segment_3d_length = (start - end).norm();
+  const double scale_p = segment_3d_length / (start_2d - end_2d).norm();
+  const double p_3d = closest_point_to_segment_2d.p * scale_p;
+  const double z_coordinate = start.z() + ((end - start).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};
+  const maliput::math::Vector3 closest_point_3d{closest_point_to_segment_2d.point.x(),
+                                                closest_point_to_segment_2d.point.y(), z_coordinate};
 
-  return {p_3d + closest_segment->p_interval.min, closest_point_3d, (xyz - closest_point_3d).norm()};
+  return {p_3d + closest_segment_3d.p_interval.min, closest_point_3d, (xyz - closest_point_3d).norm(),
+          closest_segment_3d};
 }
 
 double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, double tolerance) {
@@ -450,10 +442,12 @@ double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, double
 template maliput::math::Vector3 InterpolatedPointAtP(const LineString3d&, double, double);
 template maliput::math::Vector2 InterpolatedPointAtP(const LineString2d&, double, double);
 
-template ClosestPointResult3d GetClosestPointToSegment(const maliput::math::Vector3&, const maliput::math::Vector3&,
-                                                       const maliput::math::Vector3&, double);
-template ClosestPointResult2d GetClosestPointToSegment(const maliput::math::Vector2&, const maliput::math::Vector2&,
-                                                       const maliput::math::Vector2&, double);
+template ClosestPointToSegmentResult3d GetClosestPointToSegment(const maliput::math::Vector3&,
+                                                                const maliput::math::Vector3&,
+                                                                const maliput::math::Vector3&, double);
+template ClosestPointToSegmentResult2d GetClosestPointToSegment(const maliput::math::Vector2&,
+                                                                const maliput::math::Vector2&,
+                                                                const maliput::math::Vector2&, double);
 
 template class ClosestPointResult<maliput::math::Vector3>;
 template class ClosestPointResult<maliput::math::Vector2>;

test/geometry/utility/geometry_test.cc

@@ -391,7 +391,7 @@ INSTANTIATE_TEST_CASE_P(Get2DHeadingAtPTestGroup, Get2DHeadingAtPTest, ::testing
 struct GetClosestPointToSegmentTestCase {
   std::pair<maliput::math::Vector3, maliput::math::Vector3> segment;
   std::vector<maliput::math::Vector3> eval_points;
-  std::vector<ClosestPointResult3d> expected_closest{};
+  std::vector<ClosestPointToSegmentResult3d> expected_closest{};
 };
 
 std::vector<GetClosestPointToSegmentTestCase> GetClosestPointToSegmentTestCases() {
@@ -461,15 +461,15 @@ std::vector<GetClosestPointLineStringTestCase> GetClosestPointLineStringTestCase
       {LineString3d{{0., 0., 0.}, {10., 0., 0.}} /* line_string */,
        {{5., 5., 0.}, {-5., 5., 0.}, {10., 5., 0.}, {15., 5., 0.}} /* eval points */,
        {
-           {5., {5., 0., 0.}, 5.}, /* expected: p, point, distance */
-           {0., {0., 0., 0.}, 5. * std::sqrt(2.)},
-           {10., {10., 0., 0.}, 5.},
-           {10., {10., 0., 0.}, 5. * std::sqrt(2.)},
+           {5., {5., 0., 0.}, 5., {0, 1, {0., 10.}}}, /* expected: p, point, distance, segment */
+           {0., {0., 0., 0.}, 5. * std::sqrt(2.), {0, 1, {0., 10.}}},
+           {10., {10., 0., 0.}, 5., {0, 1, {0., 10.}}},
+           {10., {10., 0., 0.}, 5. * std::sqrt(2.), {0, 1, {0., 10.}}},
        }},
       {LineString3d{{0., 0., 0.}, {10., 0., 0.}, {15., 0., 0.}} /* line_string */,
        {{12.5, 0., 0.}} /* eval points */,
        {
-           {12.5, {12.5, 0., 0.}, 0.}, /* expected: p, point, distance */
+           {12.5, {12.5, 0., 0.}, 0., {1, 2, {10., 15.}}}, /* expected: p, point, distance, segment */
        }},
       {LineString3d{{-2., -4., -6.},
                     {0., -2., -4.},
@@ -479,10 +479,12 @@ std::vector<GetClosestPointLineStringTestCase> GetClosestPointLineStringTestCase
                     {40., 20., 10.}} /* line_string */,
        {{-1., -3., -5.}, {5., 5., 5.}} /* eval points */,
        {
-           {std::sqrt(3.), {-1., -3., -5.}, 0.}, /* expected: p, point, distance */
+           {std::sqrt(3.), {-1., -3., -5.}, 0., {0, 1, {0., 3.4641016151377544}}}, /* expected: p, point, distance,
+                                                                                      segment */
            {std::sqrt(2. * 2. + 2. * 2. + 2. * 2.) + std::sqrt(2. * 2. + 4. * 4.) + 5.,
             {5., 0., 0.},
-            5. * std::sqrt(2.)},
+            5. * std::sqrt(2.),
+            {2, 3, {7.936237570137334, 17.936237570137333}}},
        }},
   };
 }
@@ -500,6 +502,10 @@ TEST_P(GetClosestPointLineStringTest, Test) {
     EXPECT_NEAR(case_.expected_closest[i].p, dut.p, kTolerance);
     EXPECT_TRUE(maliput::math::test::CompareVectors(case_.expected_closest[i].point, dut.point, kTolerance));
     EXPECT_NEAR(case_.expected_closest[i].distance, dut.distance, kTolerance);
+    EXPECT_EQ(case_.expected_closest[i].segment.idx_start, dut.segment.idx_start);
+    EXPECT_EQ(case_.expected_closest[i].segment.idx_end, dut.segment.idx_end);
+    EXPECT_NEAR(case_.expected_closest[i].segment.p_interval.min, dut.segment.p_interval.min, kTolerance);
+    EXPECT_NEAR(case_.expected_closest[i].segment.p_interval.max, dut.segment.p_interval.max, kTolerance);
   }
 }
 
@@ -524,8 +530,12 @@ std::vector<GetClosestPointLineStringTestCase> GetClosestPointIn2dLineStringTest
       {LineString3d{{0., 0., 0.}, {10., 0., 10.}} /* line_string */,
        {{5., 0., 0.}, {5., -2., 0.}} /* eval points */,
        {
-           {10 * std::sqrt(2.) / 2., {5., 0., 5.}, 5.},                /* expected: p, point, distance */
-           {10 * std::sqrt(2.) / 2., {5., 0., 5.}, 5.385164807134504}, /* expected: p, point, distance */
+           {10 * std::sqrt(2.) / 2., {5., 0., 5.}, 5., {0, 1, {0., 10. * std::sqrt(2.)}}}, /* expected: p, point,
+                                                                                              distance, segment */
+           {10 * std::sqrt(2.) / 2.,
+            {5., 0., 5.},
+            5.385164807134504,
+            {0, 1, {0., 10. * std::sqrt(2.)}}}, /* expected: p, point, distance, segment */
        }},
   };
 }
@@ -543,6 +553,10 @@ TEST_P(GetClosestPointIn2dLineStringTest, Test) {
     EXPECT_NEAR(case_.expected_closest[i].p, dut.p, kTolerance);
     EXPECT_TRUE(maliput::math::test::CompareVectors(case_.expected_closest[i].point, dut.point, kTolerance));
     EXPECT_NEAR(case_.expected_closest[i].distance, dut.distance, kTolerance);
+    EXPECT_EQ(case_.expected_closest[i].segment.idx_start, dut.segment.idx_start);
+    EXPECT_EQ(case_.expected_closest[i].segment.idx_end, dut.segment.idx_end);
+    EXPECT_NEAR(case_.expected_closest[i].segment.p_interval.min, dut.segment.p_interval.min, kTolerance);
+    EXPECT_NEAR(case_.expected_closest[i].segment.p_interval.max, dut.segment.p_interval.max, kTolerance);
   }
 }