Struct Constraints 

pub struct Constraints { /* private fields */ }

Constraint storage and query object used by TOPP/COPP solvers.

Mathematical symbols (with code mapping)

For each path station $s_k$:

• $a_k = \dot{s}_k^2$ (squared path speed), mapped to code symbol a[k].
• $b_k = \ddot{s}_k = \frac{1}{2}\frac{\mathrm{d}a}{\mathrm{d}s}(s_k)$ (path acceleration), mapped to b[k].
• $c_k = \frac{\dddot{s}_k}{\dot{s}_k} = \frac{\mathrm{d}b}{\mathrm{d}s}(s_k)$ (normalized jerk term), mapped to c[k].

Constraint families

• First-order: 0 <= a[k] <= amax[k]
• Second-order: acc_a[k]*a[k] + acc_b[k]*b[k] <= acc_max[k]
• Third-order (nonlinear): sqrt(a[k])*(jerk_a[k]*a[k] + jerk_b[k]*b[k] + jerk_c[k]*c[k] + jerk_d[k]) <= jerk_max[k]
• Third-order (linearized): jerk_a_linear[k]*a[k] + jerk_b[k]*b[k] + jerk_c[k]*c[k] <= jerk_max_linear[k]

Storage model

All station-wise arrays are stored as circular column-major matrices. Logical station index range is [idx_s, idx_s + len), and logical column i maps to physical column (head_col + i) % capacity_col.

API contract

• get_* methods are safe public accessors and return Result<_, ConstraintError>.
• *_unchecked methods are internal fast-path helpers. Callers must satisfy preconditions; debug builds assert them.

Implementations

impl Constraints

pub const DEFAULT_CAPACITY: usize = 1000

pub fn len(&self) -> usize

Current number of logical stations stored in the buffer.

pub fn is_empty(&self) -> bool

Whether no logical stations are currently stored.

pub fn get_s(&self, idx_s: usize) -> Result<f64, ConstraintError>

Get station value s[idx_s] with bounds validation.

Errors

Returns ConstraintError::OutOfSBounds if idx_s is outside [idx_s_start(), idx_s_end()).

pub fn amax_rows(&self) -> usize

Number of rows currently allocated for first-order constraints.

Normally this is 1, but the method is intentionally generic.

pub fn acc_rows(&self) -> usize

Number of rows currently allocated for second-order constraints.

pub fn jerk_rows(&self) -> usize

Number of rows currently allocated for third-order constraints.

pub fn s_vec( &self, idx_s_from: usize, idx_s_to: usize, ) -> Result<Vec<f64>, ConstraintError>

Export station values in half-open interval [idx_s_from, idx_s_to).

Parameters

• idx_s_from: global start station id (inclusive).
• idx_s_to: global end station id (exclusive).

Returns

A contiguous vector of station values with length idx_s_to - idx_s_from.

Errors

• ConstraintError::EmptyInterval when idx_s_from >= idx_s_to.
• ConstraintError::OutOfSBounds if the interval is outside stored data.

pub fn amax_vec( &self, idx_s_from: usize, idx_s_to: usize, ) -> Result<Vec<f64>, ConstraintError>

Export first-order upper bounds in [idx_s_from, idx_s_to).

Semantics and error behavior are identical to s_vec().

pub fn get_amax(&self, idx_s: usize) -> Result<f64, ConstraintError>

Get first-order upper bound amax[idx_s] with bounds validation.

Errors

Returns ConstraintError::OutOfSBounds if idx_s is invalid.

pub fn idx_s_start(&self) -> usize

Global start station id (inclusive) of current logical window.

pub fn idx_s_end(&self) -> usize

Global end station id (exclusive) of current logical window.

pub fn get_acc_constraints<'a>( &'a self, idx_s: usize, ) -> Result<(InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>), ConstraintError>

Get second-order row views at station idx_s.

Returns

(acc_a_col, acc_b_col, acc_max_col) where each matrix is a valid_rows x 1 view into internal storage.

Errors

Returns ConstraintError::OutOfSBounds if idx_s is invalid.

pub fn get_jerk_constraints<'a>( &'a self, idx_s: usize, ) -> Result<(InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>), ConstraintError>

Get nonlinear third-order row views at station idx_s.

Returns

(jerk_a, jerk_b, jerk_c, jerk_d, jerk_max), each a valid_rows x 1 view.

Errors

Returns ConstraintError::OutOfSBounds if idx_s is invalid.

pub fn get_jerk_linear_constraints<'a>( &'a self, idx_s: usize, ) -> Result<(InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>), ConstraintError>

Get linearized third-order row views at station idx_s.

Returns

(jerk_a_linear, jerk_b, jerk_c, jerk_max_linear), each a valid_rows x 1 view into internal storage.

Errors

• ConstraintError::OutOfSBounds if idx_s is outside current station window.
• ConstraintError::LinearJerkNotAvailable if idx_s is not covered by the latest linearization interval.

pub fn expand_capacity(&mut self, new_capacity: usize)

Ensure buffer capacity is at least new_capacity columns.

Growth strategy

If expansion is required, target capacity is max(new_capacity, 2 * current_capacity + 1).

Guarantees

• Logical order of existing data is preserved.
• head_col is reset to 0 after re-layout.
• All backing matrices (s, derivative buffers, and constraint buffers) are expanded consistently.

pub fn with_constraint_1order<T: AsInputMatrix1D + ?Sized>( &mut self, amax_new: &T, idx_s: usize, ) -> Result<(), ConstraintError>

Add / tighten first-order bound amax over an interval.

Parameters

• amax_new: candidate upper bounds as R x N; each column is reduced to its minimum before being fused into storage.
• idx_s: global start station id.

Fusion rule

Stored value is updated as self.amax = min(self.amax, amax_new_reduced).

Errors

• ConstraintError::OutOfSBounds if interval is invalid.
• ConstraintError::NonPositiveA if any reduced bound is non-positive.

pub fn with_constraint_2order( &mut self, acc_a_new: &InputMatrix<'_>, acc_b_new: &InputMatrix<'_>, acc_max_new: &InputMatrix<'_>, idx_s: usize, is_negative: bool, ) -> Result<(), ConstraintError>

Append second-order inequality rows over station interval starting at idx_s.

Model

For each station column, rows satisfy: acc_a * a + acc_b * b <= acc_max.

Parameters

• acc_a_new, acc_b_new, acc_max_new: same-shape matrices (R x N).
• idx_s: global start station id.
• is_negative: whether to negate inserted rows (used to build symmetric upper/lower bounds from one physical expression).

Behavior

• Increases row counts by R on affected stations.
• Appends new rows below existing rows per station.
• Merges adjacent validity intervals when row counts match.

Errors

• ConstraintError::NoMatchDimensions for shape mismatch.
• ConstraintError::OutOfSBounds for invalid interval.

pub fn with_constraint_3order( &mut self, jerk_a_new: &InputMatrix<'_>, jerk_b_new: &InputMatrix<'_>, jerk_c_new: &InputMatrix<'_>, jerk_d_new: &InputMatrix<'_>, jerk_max_new: &InputMatrix<'_>, idx_s: usize, is_negative: bool, ) -> Result<(), ConstraintError>

Append third-order nonlinear inequality rows over station interval.

Model

sqrt(a) * (jerk_a*a + jerk_b*b + jerk_c*c + jerk_d) <= jerk_max

Parameters

• jerk_*_new: same-shape matrices (R x N).
• idx_s: global start station id.
• is_negative: if true, inserted rows are sign-flipped.

Side effects

If the inserted interval overlaps current valid_ids_linear_jerk, the linearization-valid interval is cleared because source nonlinear rows changed.

Errors

• ConstraintError::NoMatchDimensions for shape mismatch.
• ConstraintError::OutOfSBounds for invalid interval.

pub fn pop_front(&mut self, mode: ModePopConstraints)

Remove a prefix of logical stations from the front.

Modes

• ModePopConstraints::CutAtIdxS(cut): keep stations with id >= cut.
• ModePopConstraints::PopNCols(n): remove first n logical stations.

Notes

• amax values in removed columns are reset to +inf.
• Valid-row maps are trimmed and re-anchored.
• idx_s increases and head_col advances accordingly.

pub fn pop_back(&mut self, mode: ModePopConstraints)

Remove a suffix of logical stations from the back.

Modes

• ModePopConstraints::CutAtIdxS(cut): keep stations with id < cut.
• ModePopConstraints::PopNCols(n): remove last n logical stations.

Notes

• amax values in removed columns are reset to +inf.
• Valid-row maps are trimmed to new right boundary.
• idx_s is unchanged; only len shrinks.

pub fn clear(&mut self, keep_idx_s: bool)

Reset logical content and validity maps.

Parameters

• keep_idx_s: when true, preserve current global station origin; otherwise reset it to 0.

Notes

amax is reinitialized to +∞; other matrices are kept allocated and may retain old values outside the active logical window.

pub fn amax_substitute( &mut self, amax_new: &[f64], idx_from: usize, ) -> Result<(), ConstraintError>

Overwrite first-order bounds in [idx_from, idx_from + amax_new.len()).

Parameters

• amax_new: replacement values.
• idx_from: global start station id.

Errors

Returns ConstraintError::OutOfSBounds if target range is invalid.