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.

BezierCurve represents a single cubic Bézier curve segment and is the fundamental building block for all Odyssey paths. Given four control points it precomputes the polynomial coefficients used by every query method, keeping per-call cost low. A HeadingInterpolator is required at construction time so that the curve knows how to report robot orientation at any point along it. Package: org.firstinspires.ftc.teamcode.odyssey.path

​
Constructor

BezierCurve(Vector2d p0, Vector2d p1, Vector2d p2, Vector2d p3, HeadingInterpolator headingInterpolator)
​
p0
Vector2d
required
The curve’s start point (anchor at t = 0). Coordinates are typically in millimetres in the field frame.
​
p1
Vector2d
required
The first control point. The curve leaves p0 travelling in the direction of p1 − p0, scaled by 3.
​
p2
Vector2d
required
The second control point. The curve arrives at p3 from the direction of p3 − p2, scaled by 3.
​
p3
Vector2d
required
The curve’s end point (anchor at t = 1).
​
headingInterpolator
HeadingInterpolator
required
Strategy object that maps a curve parameter t to a robot heading (radians). Built-in implementations include TangentInterpolator, LinearInterpolator, ConstantInterpolator, and FaceTargetInterpolator. A heading-free overload does not exist in the public API; a HeadingInterpolator is always required.
The constructor precomputes the four polynomial coefficient vectors a, b, c, d from the control points. These are reused by getPoint, getSquaredDistanceAtT, and their callers to avoid redundant arithmetic.

​
Methods

​
getPoint

public Vector2d getPoint(double t)
Returns the position on the curve at parameter t.
​
t
double
required
Curve parameter in [0, 1]. t = 0 is p0; t = 1 is p3.
​
return
Vector2d
The 2-D field position at t, computed as the standard cubic Bernstein expansion: (1−t)³·p0 + 3t(1−t)²·p1 + 3t²(1−t)·p2 + t³·p3.

​
getTangentVector

public Vector2d getTangentVector(double t)
Returns the first derivative of the curve at t — the instantaneous direction of travel. The vector is not normalised; its magnitude is proportional to speed under uniform t-parameterisation.
​
t
double
required
Curve parameter in [0, 1].
​
return
Vector2d
First derivative B'(t) = 3(1−t)²·(p1−p0) + 6t(1−t)·(p2−p1) + 3t²·(p3−p2).

​
getTangentAngle

public double getTangentAngle(double t)
Convenience wrapper that returns atan2(B'y, B'x) — the heading angle of the tangent vector in radians. If you already hold the tangent Vector2d, calling vector.getAngleFromCur() directly is more efficient.
​
t
double
required
Curve parameter in [0, 1].
​
return
double
Tangent heading angle in radians, range (−π, π].

​
getLength

public double getLength(double b)
Computes the arc length of the curve from t = 0 to t = b using 5-point Gauss–Legendre quadrature (tolerances 1e-9, up to 1 000 evaluations).
​
b
double
required
Upper limit of integration. Must be in [0, 1]. Values ≤ 0 return 0 immediately.
​
return
double
Arc length in the same units as the control points (millimetres for a standard FTC field).
For the full curve length pass b = 1. Path caches this value at construction so you don’t need to call getLength(1) repeatedly after wrapping a curve in a Path.

​
getTAtDistance

public double getTAtDistance(double dist)
Inverts the arc-length function: returns the parameter t such that getLength(t) ≈ dist. Uses a Brent root-finding solver (tolerance 1e-9, up to 1 000 iterations).
​
dist
double
required
Target arc-length distance from p0. Clamped: values ≤ 0 return 0.0; values ≥ getLength(1) return 1.0.
​
return
double
Parameter t ∈ [0, 1] such that getLength(t) ≈ dist to within 1e-9.
Each call to getTAtDistance involves numerical integration inside the Brent solver. For performance-critical loops, consider sampling the profile at construction instead of calling this method on every control loop tick.

​
getClosestT (default steps)

public double getClosestT(Vector2d pose)
Finds the curve parameter t whose point is closest to pose using a two-phase search: a coarse scan with step 0.01 followed by a fine refinement with step 1e-5 around the best candidate.
​
pose
Vector2d
required
The 2-D query position (typically the robot’s current field position).
​
return
double
Parameter t ∈ [0, 1] that minimises the squared Euclidean distance to pose.

​
getClosestT (custom steps)

public double getClosestT(Vector2d pose, double coarseStep, double fineStep)
Overload of getClosestT with explicit step sizes for both search phases.
​
pose
Vector2d
required
The 2-D query position.
​
coarseStep
double
required
Step size for the initial coarse sweep over [0, 1]. Larger values are faster but may miss narrow local minima. Default: 0.01.
​
fineStep
double
required
Step size for the local refinement window [bestT − coarseStep, bestT + coarseStep]. Default: 1e-5.
​
return
double
Parameter t ∈ [0, 1] that minimises squared distance to pose at the given resolution.

​
getSquaredDistanceAtT

public double getSquaredDistanceAtT(double t, Vector2d pose)
Returns the squared Euclidean distance between the point on the curve at t and pose. Uses the precomputed polynomial form a·t³ + b·t² + c·t + d for efficiency; avoids a sqrt compared with computing true distance.
​
t
double
required
Curve parameter in [0, 1].
​
pose
Vector2d
required
Reference position to measure distance from.
​
return
double
Squared distance |B(t) − pose|² in units².

​
getCurvature

public double getCurvature(double t)
Returns the signed curvature magnitude κ at t.
​
t
double
required
Curve parameter in [0, 1].
​
return
double
Curvature κ = |B'(t) × B''(t)| / |B'(t)|³ in units⁻¹ (e.g. mm⁻¹ for mm control points). Returns 0 if the tangent magnitude is zero (degenerate segment).

​
getSecondDerivative

public Vector2d getSecondDerivative(double t)
Returns the second derivative B''(t) of the curve.
​
t
double
required
Curve parameter in [0, 1].
​
return
Vector2d
B''(t) = 6(1−t)·(p2 − 2p1 + p0) + 6t·(p3 − 2p2 + p1).

​
getCentripetalVector

public Vector2d getCentripetalVector(double t)
Returns the component of the second derivative that is orthogonal to the tangent — i.e. the centripetal acceleration direction. This is used by VelocityProfile to enforce lateral acceleration limits.
​
t
double
required
Curve parameter in [0, 1].
​
return
Vector2d
B''(t) − (B''·B' / |B'|²)·B' — the rejection of B'' from the tangent direction.

​
getHeadingAtT

public double getHeadingAtT(double t)
Delegates to the HeadingInterpolator provided at construction. The interpolator receives both t and a reference to this curve so it can query any geometric property it needs.
​
t
double
required
Curve parameter in [0, 1].
​
return
double
Robot heading in radians as determined by the active HeadingInterpolator.

​
Heading Interpolation

HeadingInterpolator is a single-method interface: 
// org.firstinspires.ftc.teamcode.odyssey.path.heading
public interface HeadingInterpolator {
    double getHeading(double t, BezierCurve curve);
}
Four built-in implementations ship with Odyssey:
ClassBehaviour
TangentInterpolatorHeading tracks the curve’s tangent angle — robot always faces its direction of travel.
LinearInterpolator(startHeading, endHeading)Linearly interpolates heading from startHeading at t=0 to endHeading at t=1, taking the shortest angular path.
ConstantInterpolator(heading)Holds a fixed heading for the entire segment.
FaceTargetInterpolator(target)Robot always faces a fixed 2-D field point (atan2 from current curve position).

​
Example

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

// Arch-shaped curve: start at origin, end at (1800, 0), hump at y=1200
BezierCurve curve = new BezierCurve(
    new Vector2d(0, 0),
    new Vector2d(0, 1200),
    new Vector2d(1800, 1200),
    new Vector2d(1800, 0),
    new TangentInterpolator()
);

// Total arc length
double len = curve.getLength(1);

// Parameter at the arc-length midpoint
double t = curve.getTAtDistance(len / 2);

// World position at that parameter
Vector2d mid = curve.getPoint(t);

// Curvature and heading at the same point
double kappa   = curve.getCurvature(t);
double heading = curve.getHeadingAtT(t);

// Project a robot pose onto the curve
double closestT = curve.getClosestT(new Vector2d(900, 600));

Build docs developers (and LLMs) love

Get started for freeTalk to us