Skip to content

Commit

Permalink
Addresses comments.
Browse files Browse the repository at this point in the history
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
  • Loading branch information
francocipollone committed Feb 6, 2023
1 parent 9f81200 commit 4b03a2b
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 10 deletions.
10 changes: 5 additions & 5 deletions include/maliput_sparse/geometry/line_string.h
Original file line number Diff line number Diff line change
Expand Up @@ -78,13 +78,13 @@ class LineString final {
/// 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, in general it is the start index + 1.
/// - 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 P value of the parametrized LineString.
/// 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 {
// Interval() = default;

/// Creates a Interval.
/// @param min_in Is the minimum value of the interval.
/// @param max_in Is the maximum value of the interval.
Expand All @@ -96,7 +96,7 @@ class LineString final {
/// @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.
/// Less than operator.
bool operator<(const Interval& rhs) const {
if (min < rhs.min) {
return max <= rhs.max ? true : false;
Expand Down
7 changes: 2 additions & 5 deletions src/geometry/utility/geometry.cc
Original file line number Diff line number Diff line change
Expand Up @@ -290,10 +290,7 @@ CoordinateT InterpolatedPointAtP(const LineString<CoordinateT>& line_string, dou
const CoordinateT& end = line_string[bound_points.idx_end];
const CoordinateT d_segment{end - start};
const double remaining_distance = p - bound_points.length;
if (remaining_distance < kEpsilon) {
return start;
}
return start + d_segment.normalized() * remaining_distance;
return remaining_distance < kEpsilon ? start : start + d_segment.normalized() * remaining_distance;
}

double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance) {
Expand All @@ -309,7 +306,7 @@ double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance)
template <typename CoordinateT>
BoundPointsResult GetBoundPointsAtP(const LineString<CoordinateT>& line_string, double p, double tolerance) {
p = maliput::common::RangeValidator::GetAbsoluteEpsilonValidator(0., line_string.length(), tolerance, kEpsilon)(p);
const auto segment = line_string.segments().at(typename LineString<CoordinateT>::Segment::Interval{p});
const auto segment = line_string.segments().at({p});
return {segment.idx_start, segment.idx_end, segment.p_interval.min};
}

Expand Down

0 comments on commit 4b03a2b

Please sign in to comment.