1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129

use std::fmt;

use serde::{Deserialize, Serialize};

/// An angle, stored in radians.
#[derive(Clone, Copy, Debug, Serialize, Deserialize, PartialEq, PartialOrd)]
pub struct Angle(f64);

impl Angle {
    pub const ZERO: Angle = Angle(0.0);

    // TODO Normalize here, and be careful about % vs euclid_rem
    pub(crate) fn new_rads(rads: f64) -> Angle {
        // Retain more precision for angles...
        Angle((rads * 10_000_000.0).round() / 10_000_000.0)
    }

    /// Create an angle in degrees.
    pub fn degrees(degs: f64) -> Angle {
        Angle::new_rads(degs.to_radians())
    }

    /// Invert the direction of this angle.
    pub fn opposite(self) -> Angle {
        Angle::new_rads(self.0 + std::f64::consts::PI)
    }

    pub(crate) fn invert_y(self) -> Angle {
        Angle::new_rads(2.0 * std::f64::consts::PI - self.0)
    }

    /// Rotates this angle by some degrees.
    pub fn rotate_degs(self, degrees: f64) -> Angle {
        Angle::new_rads(self.0 + degrees.to_radians())
    }

    /// Returns [0, 2pi)
    pub fn normalized_radians(self) -> f64 {
        if self.0 < 0.0 {
            // TODO Be more careful about how we store the angle, to make sure this works
            self.0 + (2.0 * std::f64::consts::PI)
        } else {
            self.0
        }
    }

    /// Returns [0, 360)
    pub fn normalized_degrees(self) -> f64 {
        self.normalized_radians().to_degrees()
    }

    /// Logically this returns [-180, 180], but keep in mind when we print this angle, it'll
    /// normalize to be [0, 360].
    pub fn shortest_rotation_towards(self, other: Angle) -> Angle {
        // https://math.stackexchange.com/questions/110080/shortest-way-to-achieve-target-angle
        Angle::degrees(
            ((self.normalized_degrees() - other.normalized_degrees() + 540.0) % 360.0) - 180.0,
        )
    }

    /// True if this angle is within some degrees of another, accounting for rotation
    pub fn approx_eq(self, other: Angle, within_degrees: f64) -> bool {
        // https://math.stackexchange.com/questions/110080/shortest-way-to-achieve-target-angle
        // This yields [-180, 180]
        let rotation =
            ((self.normalized_degrees() - other.normalized_degrees() + 540.0) % 360.0) - 180.0;
        rotation.abs() < within_degrees
    }

    /// I don't know how to describe what this does. Use for rotating labels in map-space and making
    /// sure the text is never upside-down.
    pub fn reorient(self) -> Angle {
        let theta = self.normalized_degrees().rem_euclid(360.0);
        let mut result = self;
        if theta > 90.0 {
            result = result.opposite();
        }
        if theta > 270.0 {
            result = result.opposite();
        }
        result
    }
}

impl fmt::Display for Angle {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "Angle({} degrees)", self.normalized_degrees())
    }
}

impl std::ops::Add for Angle {
    type Output = Angle;

    fn add(self, other: Angle) -> Angle {
        Angle::new_rads(self.0 + other.0)
    }
}

impl std::ops::Neg for Angle {
    type Output = Angle;

    fn neg(self) -> Angle {
        Angle::new_rads(-self.0)
    }
}

impl std::ops::Div<f64> for Angle {
    type Output = Angle;

    fn div(self, scalar: f64) -> Angle {
        if scalar == 0.0 {
            panic!("Can't divide {} / {}", self, scalar);
        }
        Angle::new_rads(self.0 / scalar)
    }
}

impl std::iter::Sum for Angle {
    fn sum<I>(iter: I) -> Angle
    where
        I: Iterator<Item = Angle>,
    {
        let mut sum = Angle::ZERO;
        for x in iter {
            sum = sum + x;
        }
        sum
    }
}