navcore_math/

current.rs

//! The current triangle: deriving set and drift, and steering to beat them.

use crate::angle;

/// A water current.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Current {
    /// The direction the current flows **towards**, degrees true.
    ///
    /// Note this is the opposite convention to wind, which is named by the
    /// direction it comes *from*. That is the standard marine usage and not
    /// a slip: a north-going current sets 000, a north wind blows from 000.
    pub set_deg: f64,
    /// Current speed in knots.
    pub drift_kn: f64,
}

/// The current implied by the difference between the vessel's motion
/// through the water and over the ground.
///
/// Every quantity here is measured by a different instrument -- heading
/// from the compass, speed through water from the log, course and speed
/// over ground from GPS -- so the result carries the error of all four. A
/// log reading 5% low produces a current that is not there.
#[must_use]
pub fn from_motion(heading_deg: f64, stw_kn: f64, cog_deg: f64, sog_kn: f64) -> Current {
    let (water_e, water_n) = to_vector(heading_deg, stw_kn);
    let (ground_e, ground_n) = to_vector(cog_deg, sog_kn);

    let (e, n) = (ground_e - water_e, ground_n - water_n);

    Current {
        set_deg: angle::norm_360(e.atan2(n).to_degrees()),
        drift_kn: e.hypot(n),
    }
}

/// A heading that holds a desired track through a current.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SteerSolution {
    /// The heading to steer, degrees true.
    pub heading_deg: f64,
    /// The speed over ground that results, knots.
    pub sog_kn: f64,
}

/// The heading to steer to make good `desired_track_deg` against a current,
/// and the speed over ground it yields.
///
/// Returns `None` when no heading achieves it: either the cross-track
/// component of the current exceeds the vessel's speed through the water,
/// or it can be cancelled but only while being pushed backwards along the
/// track. Both mean the leg cannot be laid at this speed, which is a real
/// answer and one the caller must show rather than round away.
#[must_use]
pub fn heading_to_steer(
    desired_track_deg: f64,
    stw_kn: f64,
    current: Current,
) -> Option<SteerSolution> {
    if stw_kn <= 0.0 {
        return None;
    }

    // Split the current into components along the desired track and across
    // it. Only the across component has to be steered against; the along
    // component is a free gain or loss of speed.
    let rel = angle::diff(current.set_deg, desired_track_deg).to_radians();
    let across = current.drift_kn * rel.sin();
    let along = current.drift_kn * rel.cos();

    let sin_offset = -across / stw_kn;
    if sin_offset.abs() > 1.0 {
        return None;
    }

    // asin picks the offset in -90..90, i.e. the solution that still points
    // up the track. The mirrored solution steers backwards and is never the
    // one wanted.
    let offset = sin_offset.asin();
    let sog = stw_kn * offset.cos() + along;
    if sog <= 0.0 {
        return None;
    }

    Some(SteerSolution {
        heading_deg: angle::norm_360(desired_track_deg + offset.to_degrees()),
        sog_kn: sog,
    })
}

/// (east, north) components of a speed on a bearing.
fn to_vector(bearing_deg: f64, speed_kn: f64) -> (f64, f64) {
    let b = bearing_deg.to_radians();
    (speed_kn * b.sin(), speed_kn * b.cos())
}

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

    fn close(a: f64, b: f64, tol: f64) -> bool {
        (a - b).abs() < tol
    }

    #[test]
    fn no_current_when_ground_and_water_motion_agree() {
        let c = from_motion(90.0, 6.0, 90.0, 6.0);
        assert!(close(c.drift_kn, 0.0, 1e-9), "{c:?}");
    }

    #[test]
    fn a_following_current_shows_as_extra_speed() {
        // Heading and making good 000, but 2 kn faster than the log says.
        let c = from_motion(0.0, 5.0, 0.0, 7.0);
        assert!(close(c.drift_kn, 2.0, 1e-9), "{c:?}");
        assert!(close(c.set_deg, 0.0, 1e-9), "{c:?}");
    }

    #[test]
    fn a_foul_current_shows_as_lost_speed_setting_astern() {
        let c = from_motion(0.0, 5.0, 0.0, 3.0);
        assert!(close(c.drift_kn, 2.0, 1e-9), "{c:?}");
        assert!(close(c.set_deg, 180.0, 1e-9), "{c:?}");
    }

    #[test]
    fn a_beam_current_shows_in_the_leeway_between_heading_and_cog() {
        // Steering 000 at 5 kn but making good 045 at 5*sqrt(2)/... -- pick
        // the clean case: 5 north plus 5 east gives 045 at 7.071.
        let c = from_motion(0.0, 5.0, 45.0, 7.0710678);
        assert!(close(c.set_deg, 90.0, 1e-6), "{c:?}");
        assert!(close(c.drift_kn, 5.0, 1e-6), "{c:?}");
    }

    #[test]
    fn steering_offsets_upstream_of_the_track() {
        // Track due north, current setting east: crab to the east... no --
        // steer *into* the current, so west of north.
        let c = Current { set_deg: 90.0, drift_kn: 2.0 };
        let s = heading_to_steer(0.0, 6.0, c).unwrap();
        assert!(s.heading_deg > 270.0, "should steer west of north: {s:?}");
        // sin(offset) = -2/6, offset = -19.47 deg
        assert!(close(s.heading_deg, 360.0 - 19.4712, 1e-3), "{s:?}");
        assert!(close(s.sog_kn, 6.0 * (19.4712_f64.to_radians()).cos(), 1e-6));
    }

    #[test]
    fn steering_and_deriving_are_inverses() {
        let c = Current { set_deg: 210.0, drift_kn: 1.7 };
        let track = 55.0;
        let s = heading_to_steer(track, 6.0, c).unwrap();
        // Sail the solution and the resulting ground motion must be the
        // track we asked for.
        let derived = from_motion(s.heading_deg, 6.0, track, s.sog_kn);
        assert!(close(derived.set_deg, c.set_deg, 1e-6), "{derived:?}");
        assert!(close(derived.drift_kn, c.drift_kn, 1e-6), "{derived:?}");
    }

    #[test]
    fn a_current_stronger_than_the_boat_across_the_track_has_no_solution() {
        let c = Current { set_deg: 90.0, drift_kn: 5.0 };
        assert!(heading_to_steer(0.0, 4.0, c).is_none());
    }

    #[test]
    fn cancelling_the_cross_component_while_swept_backwards_has_no_solution() {
        // Current dead against the track and faster than the boat: the
        // across component is zero so the naive solve succeeds, but the
        // vessel goes backwards. Must be rejected.
        let c = Current { set_deg: 180.0, drift_kn: 8.0 };
        assert!(heading_to_steer(0.0, 4.0, c).is_none());
    }

    #[test]
    fn a_stopped_vessel_cannot_steer() {
        let c = Current { set_deg: 90.0, drift_kn: 1.0 };
        assert!(heading_to_steer(0.0, 0.0, c).is_none());
    }
}