raceline-optimizer 0.2.1

Racing-line and minimum-time trajectory optimization for cars and motorcycles
Documentation
use crate::contracts::Point2;

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct SectionFrameProgress {
    pub det_geom: f64,
    pub forward_progress_per_speed: f64,
    pub s_dot: f64,
    pub n_dot: f64,
    pub sigma_dt_ds: f64,
    pub dn_ds: f64,
}

#[must_use]
pub fn section_frame_progress(
    n_m: f64,
    v_mps: f64,
    beta_rad: f64,
    xi_rad: f64,
    tangent: Point2,
    left_normal: Point2,
    section_dir: Point2,
    section_dir_ds: Point2,
) -> SectionFrameProgress {
    section_frame_progress_from_derivatives(
        n_m,
        v_mps,
        beta_rad,
        xi_rad,
        tangent,
        left_normal,
        tangent,
        section_dir,
        section_dir_ds,
    )
}

#[must_use]
pub fn section_frame_progress_from_derivatives(
    n_m: f64,
    v_mps: f64,
    beta_rad: f64,
    xi_rad: f64,
    tangent: Point2,
    left_normal: Point2,
    centerline_ds: Point2,
    section_dir: Point2,
    section_dir_ds: Point2,
) -> SectionFrameProgress {
    let theta = xi_rad + beta_rad;
    let vel_dir = [
        theta.cos() * tangent[0] + theta.sin() * left_normal[0],
        theta.cos() * tangent[1] + theta.sin() * left_normal[1],
    ];
    let p_s = [
        centerline_ds[0] - n_m * section_dir_ds[0],
        centerline_ds[1] - n_m * section_dir_ds[1],
    ];
    let p_n = [-section_dir[0], -section_dir[1]];
    let det_geom = cross2(p_s, p_n);
    let det_for_division = signed_max_abs(det_geom, 1e-9);
    let forward_progress_per_speed = cross2(vel_dir, p_n) / det_for_division;
    let speed = v_mps.max(1e-6);
    let s_dot = speed * forward_progress_per_speed;
    let n_dot = speed * cross2(p_s, vel_dir) / det_for_division;
    let sigma_dt_ds = (1.0 / signed_max_abs(s_dot, 1e-9)).max(1e-9);

    SectionFrameProgress {
        det_geom,
        forward_progress_per_speed,
        s_dot,
        n_dot,
        sigma_dt_ds,
        dn_ds: sigma_dt_ds * n_dot,
    }
}

#[must_use]
pub fn pure_frenet_path_factor(n_m: f64, kappa_1pm: f64) -> f64 {
    1.0 - n_m * kappa_1pm
}

#[must_use]
pub fn heading_forward_projection(beta_rad: f64, xi_rad: f64) -> f64 {
    (beta_rad + xi_rad).cos()
}

#[must_use]
pub fn velocity_heading_curvature_1pm(
    v_mps: f64,
    omega_z_radps: f64,
    dbeta_ds: f64,
    sigma_dt_ds: f64,
) -> f64 {
    let speed = signed_max_abs(v_mps, 1e-6);
    let beta_dot_radps = dbeta_ds / signed_max_abs(sigma_dt_ds, 1e-9);
    (omega_z_radps + beta_dot_radps) / speed
}

#[must_use]
pub fn cross2(left: Point2, right: Point2) -> f64 {
    left[0] * right[1] - left[1] * right[0]
}

#[must_use]
pub fn signed_max_abs(value: f64, min_abs: f64) -> f64 {
    if value.abs() < min_abs {
        min_abs.copysign(value)
    } else {
        value
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn section_frame_progress_matches_pure_frenet_when_section_is_right_normal() {
        let progress = section_frame_progress(
            0.4,
            20.0,
            0.02,
            0.03,
            [1.0, 0.0],
            [0.0, 1.0],
            [0.0, -1.0],
            [0.1, 0.0],
        );

        let expected_factor = pure_frenet_path_factor(0.4, 0.1);
        let expected_forward = (0.02_f64 + 0.03).cos();
        assert!((progress.det_geom - expected_factor).abs() < 1.0e-12);
        assert!(
            (progress.forward_progress_per_speed - expected_forward / expected_factor).abs()
                < 1.0e-12
        );
        assert!(
            (progress.sigma_dt_ds - expected_factor / (20.0 * expected_forward)).abs() < 1.0e-12
        );
    }

    #[test]
    fn section_frame_can_be_regular_when_pure_frenet_factor_is_negative() {
        let pure = pure_frenet_path_factor(-4.8, -0.25);
        let progress = section_frame_progress(
            -4.8,
            12.0,
            0.0,
            0.0,
            [1.0, 0.0],
            [0.0, 1.0],
            [0.0, -1.0],
            [-0.06, 0.0],
        );

        assert!(pure < 0.0);
        assert!(progress.det_geom > 0.0);
        assert!(progress.forward_progress_per_speed > 0.0);
        assert!(progress.sigma_dt_ds.is_finite());
    }

    #[test]
    fn velocity_heading_curvature_includes_sideslip_heading_rate() {
        let curvature = velocity_heading_curvature_1pm(10.0, 0.2, 0.03, 0.1);

        assert!((curvature - 0.05).abs() < 1.0e-12);
    }
}