11#pragma once
22#include " RaidZeroLib/api/Geometry/Point.hpp"
3- #include < algorithm>
4- #include < exception>
53#include < vector>
4+ #include < ranges>
5+ #include < concepts>
6+ #include < cassert>
67
78namespace rz {
8- using namespace okapi ;
99
10+
11+ /* *
12+ * @brief Immutable sequence of 2D waypoints with unit-safe geometry.
13+ *
14+ * `DiscretePath` stores a read-only ordered list of `Point` (meters). It supports:
15+ * - Construction from an initializer list or any C++20 range of points,
16+ * - Concatenation with another path or a single point,
17+ * - Const iteration (forward and reverse),
18+ * - Random access and front/back access,
19+ * - Geometric queries such as signed curvature at an interior vertex.
20+ *
21+ * @invariant Paths are **non-empty**. All constructors assert `size() > 0`.
22+ * @note This type is immutable: iteration returns const iterators and no mutating
23+ * accessors are exposed.
24+ */
1025class DiscretePath {
1126 public:
12- DiscretePath () = default ;
13- DiscretePath (
const std::initializer_list<Point>& waypoint);
14- DiscretePath (
const std::vector<Point>& waypoint);
15- ~DiscretePath () = default ;
27+ using value_type = Point;
28+ using const_reference = const Point&;
29+ using container_type = std::vector<Point>;
30+ using size_type = std::size_t ;
31+ using const_iterator = std::vector<Point>::const_iterator;
32+ using const_reverse_iterator = std::vector<Point>::const_reverse_iterator;
33+
34+ /* *
35+ * @brief Constructs a path from a brace-initialized list of points.
36+ *
37+ * @param waypoints Non-empty list of waypoints (meters).
38+ * @pre `waypoints.size() > 0` (debug-checked).
39+ */
40+ explicit DiscretePath (std::initializer_list<Point> waypoints);
1641
42+ /* *
43+ * @brief Constructs a path from any C++20 input range of points.
44+ *
45+ * Accepts containers and views whose element/reference type is convertible to `Point`,
46+ * e.g. `std::vector<Point>`, `std::list<Point>`, raw arrays, or range pipelines
47+ * like `points | std::views::filter(...)`.
48+ *
49+ * @tparam Range A `std::ranges::input_range` whose reference is convertible to `Point`.
50+ * @param range Non-empty range of waypoints (meters).
51+ * @pre `std::ranges::distance(range) > 0` (debug-checked after construction).
52+ * @note This constructor is disabled for `Range = DiscretePath` to avoid hijacking copy/move.
53+ */
54+ template <std::ranges::input_range Range>
55+ requires (
56+ !std::same_as<std::remove_cvref_t <Range>, DiscretePath> &&
57+ std::convertible_to<std::ranges::range_reference_t <Range>, Point>
58+ )
59+ explicit DiscretePath (Range&& range)
60+ : path(std::ranges::begin(range), std::ranges::end(range)) {
61+ assert (!path.empty () && " DiscretePath cannot be empty"
62+ }
63+
64+ /* *
65+ * @brief Concatenates two paths (this followed by @p rhs).
66+ *
67+ * @param rhs Path to append.
68+ * @return New path containing `*this` then `rhs`.
69+ */
1770 DiscretePath operator +(const DiscretePath& rhs) const ;
71+
72+ /* *
73+ * @brief Appends a single waypoint to the end of the path.
74+ *
75+ * @param rhs Point to append (meters).
76+ * @return New path with @p rhs appended.
77+ */
1878 DiscretePath operator +(const Point& rhs) const ;
19- DiscretePath& operator +=(const DiscretePath& rhs);
20- DiscretePath& operator +=(const Point& rhs);
21-
22- std::vector<Point>::iterator begin ();
23- std::vector<Point>::const_iterator begin () const ;
24- std::vector<Point>::iterator end ();
25- std::vector<Point>::const_iterator end () const ;
26-
27- std::vector<Point>::reverse_iterator rbegin ();
28- std::vector<Point>::const_reverse_iterator rbegin () const ;
29- std::vector<Point>::reverse_iterator rend ();
30- std::vector<Point>::const_reverse_iterator rend () const ;
31-
32- Point& operator [](int index);
33- const Point& operator [](int index) const ;
34- Point& front ();
35- const Point& front () const ;
36- Point& back ();
37- const Point& back () const ;
38- QCurvature getCurvature (int index) const ;
39- int size () const ;
79+
80+ // / @brief Forward begin iterator.
81+ const_iterator begin () const noexcept ;
82+
83+ // / @brief Forward end iterator (one past the last).
84+ const_iterator end () const noexcept ;
85+
86+ // / @brief Reverse begin iterator (last element).
87+ const_reverse_iterator rbegin () const noexcept ;
88+
89+ // / @brief Reverse end iterator (one before the first).
90+ const_reverse_iterator rend () const noexcept ;
91+
92+ /* *
93+ * @brief Random access without bounds checking.
94+ * @param index Zero-based index in `[0, size())`.
95+ * @return Reference to the indexed point.
96+ */
97+ const_reference operator [](size_type index) const noexcept ;
98+
99+ // / @brief First waypoint.
100+ const_reference front () const noexcept ;
101+
102+ // / @brief Last waypoint.
103+ const_reference back () const noexcept ;
104+
105+ /* *
106+ * @brief Signed curvature at vertex @p index using the circumcircle of three consecutive points.
107+ *
108+ * For an interior index `i` (i.e. `1 ≤ i ≤ size()-2`), let
109+ * `R = circumradius(P[i-1], P[i], P[i+1])`. The unsigned curvature is `κ = 1/R`.
110+ * The sign follows the triangle orientation (left turn positive, right turn negative).
111+ * Endpoints and collinear triples yield 0.
112+ *
113+ * @param index Vertex index at which to evaluate curvature.
114+ * @return Curvature in 1/meters (`au::QuantityD<au::Inverse<au::Meters>>`).
115+ */
116+ au::QuantityD<au::Inverse<au::Meters>> getCurvature (std::size_t index) const noexcept ;
117+
118+ // / @brief Number of waypoints.
119+ size_type size () const noexcept ;
40120
41121 private:
42122 std::vector<Point> path;
43123};
44124
45- std::vector<Translation>::const_iterator closestPoint (std::vector<Translation>::const_iterator begin,
46- std::vector<Translation>::const_iterator end, const Point& point);
47- } // namespace rz
125+ /* *
126+ * @brief Iterator-based closest-point search by Euclidean distance.
127+ *
128+ * Returns an iterator to the element in `[begin, end)` minimizing `distTo(point)`.
129+ * If the range is empty, returns `end`.
130+ *
131+ * @param begin Const iterator to the first waypoint.
132+ * @param end Const iterator one past the last waypoint.
133+ * @param point Query point (meters).
134+ * @return Iterator to the closest waypoint, or @p end if the range is empty.
135+ */
136+ DiscretePath::const_iterator closestPoint (DiscretePath::const_iterator begin,
137+ DiscretePath::const_iterator end, const Point& point);
138+ } // namespace rz
0 commit comments