Skip to main content

Documentation Index

Fetch the complete documentation index at: https://mintlify.com/saquer916/odyssey/llms.txt

Use this file to discover all available pages before exploring further.

Path composes one or more BezierCurve segments into a single continuous path indexed by arc-length distance from the start. At construction time it caches the full arc length of every segment so that subsequent distance-based queries — position, heading, curvature, tangent, centripetal vector — each pay only the cost of locating the right segment and a single getTAtDistance call, rather than re-integrating the whole curve. Package: org.firstinspires.ftc.teamcode.odyssey.path

​
Constructor

Path(BezierCurve... curves)
​
curves
BezierCurve...
required
One or more BezierCurve segments in traversal order. Passed as a varargs list so any number of segments can be composed without building an array manually. Each segment’s full arc length (getLength(1)) is computed and cached at construction time.
Arc-length caching happens once, eagerly, during the constructor call. For long multi-segment paths with tight Gauss–Legendre tolerances this may take a few milliseconds — construct Path objects during initialisation, not inside a control loop.

​
Methods

​
getTotalLength

public double getTotalLength()
Returns the sum of the cached arc lengths of all segments.
​
return
double
Total path length in the same units as the control points (millimetres for a standard FTC field). This value is computed once at construction and returned in O(1).

​
getPointOnPath

public Pose2d getPointOnPath(double distance)
Returns the field position and robot heading at a given arc-length distance from the path start.
​
distance
double
required
Distance from the start of the path in millimetres.
​
return
Pose2d
A Pose2d containing the field-frame (x, y) position and heading (radians, normalised to (−π, π]) at the requested distance. The heading is provided by the HeadingInterpolator attached to the containing segment.
getPointOnPath clamps to path endpoints for out-of-range values: a distance ≤ 0 returns the pose at t = 0 of the first segment, and any distance beyond getTotalLength() returns the pose at t = 1 of the last segment.

​
getDistanceOnPath

public double getDistanceOnPath(Vector2d pose)
Projects a 2-D robot position onto the path and returns the corresponding arc-length distance from the start. Internally it calls getClosestT on every segment and picks the segment with the smallest squared distance.
​
pose
Vector2d
required
Current robot position in the field frame (millimetres).
​
return
double
Arc-length distance from the path start to the closest point on the path, in millimetres.
The projection sweeps all segments every call. On a path with many segments, consider limiting calls to once per control loop tick and caching the result rather than calling it multiple times per iteration.

​
getCurvatureFromPathDistance

public double getCurvatureFromPathDistance(double dist)
Returns the curvature κ at a given arc-length distance along the path.
​
dist
double
required
Distance from path start in millimetres. Values ≤ 0 return the curvature at t = 0 of the first segment; values beyond the total length return the curvature at t = 1 of the last segment.
​
return
double
Curvature κ = |B' × B''| / |B'|³ at the resolved parameter, in mm⁻¹. Returns 0 on degenerate (zero-length) segments.

​
getTangentFromPathDistance

public Vector2d getTangentFromPathDistance(double dist)
Returns the (unnormalised) tangent vector at a given arc-length distance.
​
dist
double
required
Distance from path start in millimetres. Clamped to path endpoints as with the other distance-based methods.
​
return
Vector2d
First derivative B'(t) of the containing segment at the resolved parameter. The vector magnitude reflects the parametric speed, not physical speed; normalise with vector.normalize() if you need a unit direction.

​
getCentripetalVectorPath

public Vector2d getCentripetalVectorPath(double dist)
Returns the centripetal acceleration direction at a given arc-length distance — i.e. the component of B''(t) orthogonal to the tangent. This is used by VelocityProfile to compute lateral acceleration when enforcing the centripetal limit.
​
dist
double
required
Distance from path start in millimetres. Clamped to path endpoints.
​
return
Vector2d
The centripetal vector B''(t) − (B''·B' / |B'|²)·B' at the resolved parameter.

​
Multi-Segment Example

import org.firstinspires.ftc.teamcode.odyssey.geometry.Vector2d;
import org.firstinspires.ftc.teamcode.odyssey.geometry.Pose2d;
import org.firstinspires.ftc.teamcode.odyssey.path.BezierCurve;
import org.firstinspires.ftc.teamcode.odyssey.path.Path;
import org.firstinspires.ftc.teamcode.odyssey.path.heading.LinearInterpolator;
import org.firstinspires.ftc.teamcode.odyssey.path.heading.TangentInterpolator;

// Segment 1: straight exit from start zone
BezierCurve seg1 = new BezierCurve(
    new Vector2d(0, 0),
    new Vector2d(0, 600),
    new Vector2d(0, 1200),
    new Vector2d(0, 1800),
    new TangentInterpolator()
);

// Segment 2: sweeping turn toward scoring position
BezierCurve seg2 = new BezierCurve(
    new Vector2d(0, 1800),
    new Vector2d(0, 2400),
    new Vector2d(900, 2700),
    new Vector2d(1800, 2700),
    new LinearInterpolator(Math.PI / 2, 0)
);

// Compose into a single path — segment lengths are cached here
Path path = new Path(seg1, seg2);

double total = path.getTotalLength();

// Robot pose at the arc-length midpoint
Pose2d midPose = path.getPointOnPath(total / 2);

// Project a noisy robot position back to a path distance
double robotDist = path.getDistanceOnPath(new Vector2d(50, 900));

// Curvature and tangent at 1 500 mm from start
double kappa   = path.getCurvatureFromPathDistance(1500);
Vector2d tang  = path.getTangentFromPathDistance(1500);
For smooth heading across segment boundaries, ensure the endHeading of one segment’s interpolator matches the startHeading of the next. Path applies each segment’s HeadingInterpolator independently — there is no global heading sweep across the full path.

Build docs developers (and LLMs) love

Get started for freeTalk to us