Make rest of pathing use au, improve parametric path approximation

Author: Ryan4253

CMakeLists.txt

@@ -136,9 +136,9 @@ add_executable(RaidZeroLib
         # include/RaidZeroLib/api/Utility/CrossPlatformMutex.hpp
         include/RaidZeroLib/api/Utility/Math.hpp
         include/RaidZeroLib/api/Pathing/DiscretePath.hpp
-        # include/RaidZeroLib/api/Pathing/ParametricPath.hpp
-        # include/RaidZeroLib/api/Pathing/CubicBezier.hpp
-        # include/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.hpp
+        include/RaidZeroLib/api/Pathing/ParametricPath.hpp
+        include/RaidZeroLib/api/Pathing/CubicBezier.hpp
+        include/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.hpp
         # include/RaidZeroLib/api/Trajectory/MotionProfile/TrapezoidalMotionProfile.hpp
         # include/RaidZeroLib/api/Trajectory/MotionProfile/MotionProfile.hpp
         # include/RaidZeroLib/api/Filter/SlewRate.hpp
@@ -159,10 +159,10 @@ add_executable(RaidZeroLib
         # src/RaidZeroLib/api/Utility/CrossPlatformMutex.cpp
         src/RaidZeroLib/api/Utility/Math.cpp
         # src/RaidZeroLib/api/Utility/CrossPlatformThread.cpp
-        # src/RaidZeroLib/api/Pathing/ParametricPath.cpp
+        src/RaidZeroLib/api/Pathing/ParametricPath.cpp
         src/RaidZeroLib/api/Pathing/DiscretePath.cpp
-        # src/RaidZeroLib/api/Pathing/CubicBezier.cpp
-        # src/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.cpp
+        src/RaidZeroLib/api/Pathing/CubicBezier.cpp
+        src/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.cpp
         # src/RaidZeroLib/api/Filter/SlewRate.cpp
         src/RaidZeroLib/api/Geometry/Twist.cpp
         src/RaidZeroLib/api/Geometry/Point.cpp
@@ -180,9 +180,9 @@ add_executable(RaidZeroLib
         test/test/Geometry/PoseTest.cpp
 
         test/test/Pathing/DiscretePathTest.cpp
-        # test/test/Pathing/ParametricPathTest.cpp
-        # test/test/Pathing/CubicBezierTest.cpp
-        # test/test/Pathing/PiecewiseCubicBezierTest.cpp
+        test/test/Pathing/ParametricPathTest.cpp
+        test/test/Pathing/CubicBezierTest.cpp
+        test/test/Pathing/PiecewiseCubicBezierTest.cpp
 
         # test/test/Units/UnitsTest.cpp
 

include/RaidZeroLib/api/Pathing/CubicBezier.hpp

@@ -1,37 +1,126 @@
 #pragma once
 #include "RaidZeroLib/api/Geometry/Point.hpp"
 #include "RaidZeroLib/api/Pathing/ParametricPath.hpp"
-#include "RaidZeroLib/api/Units/Units.hpp"
 
 namespace rz {
-using namespace okapi;
 
+/**
+ * @brief Cubic Bézier parametric path segment.
+ *
+ * A `CubicBezier` represents a smooth parametric curve defined by four control points:
+ * \f[
+ * P(t) = (1-t)^3 c_0 + 3(1-t)^2 t c_1 + 3(1-t)t^2 c_2 + t^3 c_3, \quad t \in [0, 1].
+ * \f]
+ *
+ * Each segment is constructed from two knots, which encode both position
+ * and tangent direction/length at their endpoints. The constructor expands those into
+ * the four cubic control points:
+ * - `c₀ = start.getPoint()`
+ * - `c₁ = start.getForwardControl()`
+ * - `c₂ = end.getBackwardControl()`
+ * - `c₃ = end.getPoint()`
+ *
+ * **Continuity:** When consecutive segments share the same end/start `Knot`,
+ * the resulting chain is automatically C⁰ and C¹ continuous at the join.
+ *
+ * @see PiecewiseCubicBezier
+ */
 class CubicBezier : public ParametricPath {
     public:
+    /**
+     * @brief Control knot defining both position and tangent handle geometry.
+     *
+     * Each `Knot` specifies a point `(x, y)` in meters, an outgoing tangent
+     * direction `theta` in radians, and a tangent handle length `magnitude` in meters.
+     * From these, two derived “handles” are computed:
+     * - Forward control: `point + magnitude * [cos(theta), sin(theta)]`
+     * - Backward control: `point - magnitude * [cos(theta), sin(theta)]`
+     *
+     * These handles are used when constructing Bézier segments:
+     * the *start* control point contributes its **forward** handle,
+     * and the *end* control point contributes its **backward** handle.
+     */
     class Knot {
         public:
-        Knot(QLength x, QLength y, QAngle theta, QLength magnitude);
+        /**
+         * @brief Constructs a control knot with position, tangent angle, and handle length.
+         *
+         * @param x X coordinate (meters).
+         * @param y Y coordinate (meters).
+         * @param theta Tangent direction (radians).
+         * @param magnitude Length of tangent handle (meters).
+         *
+         * @pre `magnitude > 0`.
+         */
+        Knot(au::QuantityD<au::Meters> x, au::QuantityD<au::Meters> y, 
+                     au::QuantityD<au::Radians> theta, au::QuantityD<au::Meters> magnitude) noexcept;
 
-        Point getPoint() const;
+        /// @brief Returns the anchor position (x, y) of the control knot.
+        Point getPoint() const noexcept;
 
-        Point getForwardControl() const;
+        /// @brief Returns the forward (outgoing) control handle in world coordinates.
+        Point getForwardControl() const noexcept;
 
-        Point getBackwardControl() const;
+        /// @brief Returns the backward (incoming) control handle in world coordinates.
+        Point getBackwardControl() const noexcept;
 
         private:
-        QLength x;
-        QLength y;
-        QAngle theta;
-        QLength magnitude;
+        au::QuantityD<au::Meters> x;
+        au::QuantityD<au::Meters> y;
+        au::QuantityD<au::Radians> theta;
+        au::QuantityD<au::Meters> magnitude;
     };
 
-    CubicBezier(Knot start, Knot end);
+    /**
+     * @brief Constructs a cubic Bézier segment between two control knots.
+     *
+     * The start and end control points define the endpoints and tangents.
+     * Internally, the four Bézier control points are expanded as:
+     * ```
+     * c0 = start.getPoint();
+     * c1 = start.getForwardControl();
+     * c2 = end.getBackwardControl();
+     * c3 = end.getPoint();
+     * ```
+     *
+     * @param start Start control knot.
+     * @param end   End control knot.
+     */
+    CubicBezier(const Knot& start, const Knot& end) noexcept;
 
-    Point getPoint(double t) const override;
+    /**
+     * @brief Computes the position P(t) along the cubic Bézier.
+     *
+     * @param t Normalized parameter ∈ [0, 1].
+     * @return 2D point (meters).
+     */
+    Point getPoint(double t) const noexcept override;
 
-    Point getVelocity(double t) const override;
+    /**
+     * @brief Computes the first derivative dP/dt at parameter t.
+     *
+     * The derivative is given by:
+     * \f[
+     * P'(t) = 3(1-t)^2 (c_1 - c_0) + 6(1-t)t (c_2 - c_1) + 3t^2 (c_3 - c_2)
+     * \f]
+     *
+     * @param t Normalized parameter ∈ [0, 1].
+     * @return Velocity vector (meters per unit t).
+     */
+    Point getVelocity(double t) const noexcept override;
 
-    Point getAcceleration(double t) const override;
+    /**
+     * @brief Computes the second derivative d²P/dt² at parameter t.
+     *
+     * The expression is:
+     * \f[
+     * P''(t) = 6(1-t)(c_2 - 2c_1 + c_0) + 6t(c_3 - 2c_2 + c_1)
+     * \f]
+     *
+     * @param t Normalized parameter ∈ [0, 1].
+     * @return Acceleration vector (meters per unit t²).
+     */
+    Point getAcceleration(double t) const noexcept override;
 
     private:
     Point c0;
@@ -40,4 +129,4 @@ class CubicBezier : public ParametricPath {
     Point c3;
 };
 
-} // namespace rz
+} // namespace rz

include/RaidZeroLib/api/Pathing/ParametricPath.hpp

@@ -1,40 +1,125 @@
 #pragma once
 #include "RaidZeroLib/api/Pathing/DiscretePath.hpp"
+#include "au/au.hpp"
 
 namespace rz {
-using namespace okapi;
 
+
+/**
+ * @brief Differentiable 2D path over a unit-interval parameter t ∈ [0, 1].
+ *
+ * Implementers provide position and its first two derivatives with respect to the
+ * *dimensionless* path parameter @p t. Higher-level queries—heading, curvature,
+ * arclength, parameter stepping by distance, and discretization—are derived here.
+ *
+ * **Units & semantics**
+ * - `getPoint(t)` → position in meters (`Point` stores meter-valued components).
+ * - `getVelocity(t)` → dP/dt in meters (derivative w.r.t. parameter, not time).
+ * - `getAcceleration(t)` → d²P/dt² in meters (w.r.t. parameter).
+ * - `getTheta(t)` → heading of the tangent (radians).
+ * - `getCurvature(t)` → signed curvature κ = wedge(dP/dt, d²P/dt²) / |dP/dt|³ (1/meters).
+ * - `getLength(a,b)` → ∫_a^b |dP/dt| dt (meters).
+ *
+ * **Domain**: All functions expect 0 ≤ t ≤ 1 (implementations may clamp internally).
+ *
+ * **Continuity**: For stable curvature, the path should be at least C¹ and piecewise C².
+ */
 class ParametricPath {
     public:
-    virtual QLength getX(double t) const;
-
-    virtual QLength getY(double t) const;
-
-    virtual QLength getdX(double t) const;
-
-    virtual QLength getdY(double t) const;
-
-    virtual QLength getddX(double t) const;
-
-    virtual QLength getddY(double t) const;
-
-    virtual Point getPoint(double t) const = 0;
-
-    virtual Point getVelocity(double t) const = 0;
-
-    virtual Point getAcceleration(double t) const = 0;
-
-    virtual Rotation getTheta(double t) const;
-
-    virtual QCurvature getCurvature(double t) const;
-
-    virtual QLength getLength(double tStart = 0, double tEnd = 1) const;
-
-    virtual DiscretePath toDiscrete(int numSegments, bool end = true) const;
-
-    virtual DiscretePath toDiscrete(QLength distance, bool end = true) const;
-
-    virtual double stepT(double t, QLength distance) const;
+    /// @brief Virtual destructor.
+    virtual ~ParametricPath() = default;
+
+    /**
+     * @brief Position on the path at parameter @p t.
+     *
+     * @param t Dimensionless parameter, typically in [0, 1].
+     * @return 2D point in meters.
+     *
+     * @pre 0 ≤ t ≤ 1.
+     */
+    virtual Point getPoint(double t) const noexcept = 0;
+
+    /**
+     * @brief First derivative dP/dt at parameter @p t (w.r.t. path parameter).
+     *
+     * @param t Dimensionless parameter.
+     * @return Tangent vector; magnitude has units of meters.
+     *
+     * @note This is not velocity w.r.t. physical time.
+     *
+     * @pre 0 ≤ t ≤ 1.
+     */
+    virtual Point getVelocity(double t) const noexcept = 0;
+
+    /**
+     * @brief Second derivative d²P/dt² at parameter @p t (w.r.t. path parameter).
+     *
+     * @param t Dimensionless parameter.
+     * @return Second derivative vector; components in meters.
+     *
+     * @pre 0 ≤ t ≤ 1.
+     */
+    virtual Point getAcceleration(double t) const noexcept = 0;
+
+    /**
+     * @brief Path heading: orientation of the tangent dP/dt.
+     *
+     * Equivalent to `atan2(dy/dt, dx/dt)` via `Point::theta()`.
+     *
+     * @param t Dimensionless parameter.
+     * @return Heading angle in radians.
+     *
+     * @pre 0 ≤ t ≤ 1.
+     *
+     * @note Returns 0 when |dP/dt| ≈ 0 (stationary/cusp).
+     */
+    virtual au::QuantityD<au::Radians> getTheta(double t) const noexcept;
+
+
+    /**
+     * @brief Signed curvature κ(t) = wedge(dP/dt, d²P/dt²) / |dP/dt|³.
+     *
+     * Positive for left-turning, negative for right-turning (right-handed coordinates).
+     *
+     * @param t Dimensionless parameter.
+     * @return Curvature in 1/meters.
+     *
+     * @pre 0 ≤ t ≤ 1.
+     *
+     * @note Returns 0 when |dP/dt| is ~0 to avoid division by zero.
+     */
+    virtual au::QuantityD<au::Inverse<au::Meters>> getCurvature(double t) const noexcept;
+
+    /**
+     * @brief Arclength along the path between parameters @p tStart and @p tEnd.
+     *
+     * Computes s = ∫_{tStart}^{tEnd} |dP/dt| dt using adaptive Gauss–Legendre quadrature.
+     * https://en.wikipedia.org/wiki/Adaptive_quadrature
+     *
+     * @param tStart Start parameter (default 0).
+     * @param tEnd   End parameter (default 1).
+     * @return Length in meters.
+     *
+     * @pre 0 ≤ tStart ≤ tEnd ≤ 1.
+     */
+    virtual au::QuantityD<au::Meters> getLength(double tStart = 0, double tEnd = 1) const noexcept;
+
+    /**
+     * @brief Advance the parameter by a target arclength measured along the path.
+     *
+     * Finds t' ≥ t such that ∫_{t}^{t'} |dP/dt| dt ≈ @p distance. Uses a safeguarded
+     * Newton/bisection refinement with bracketing and clamps to t' ≤ 1.
+     * https://discourse.julialang.org/t/references-and-implementation-for-safe-hybrid-univariate-newtons-method/87638
+     *
+     * @param t        Current parameter in [0, 1].
+     * @param distance Desired arclength step (meters), s ≥ 0.
+     * @return Next parameter t' ∈ [t, 1].
+     *
+     * @note If @p distance == 0 or t == 1, returns 1.
+     */
+    virtual double stepT(double t, au::QuantityD<au::Meters> distance) const noexcept;
+
+    virtual DiscretePath toDiscrete(au::QuantityD<au::Meters> distance, bool includeEnd = true) const;
 };
 
-} // namespace rz
+} // namespace rz