integer_angles/
angle.rs

1use nalgebra::{convert, try_convert, RealField};
2use serde_derive::{Deserialize, Serialize};
3use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
4
5/// Implements an angle structure where angles are stored as integers.
6///
7/// Also keeps track of if the angle is going clockwise or counter-clockwise.  It also stores
8/// a full circle different from a 0 degree arc.  This gets rid of precision issues when
9/// storing angles as `pi` can be represented exactly if you change the units.
10#[derive(Copy, Clone, Eq, PartialEq, Debug, Default, Serialize, Deserialize)]
11pub struct Angle {
12    clockwise: bool,
13    units: Option<u64>,
14}
15
16use quickcheck::{Arbitrary, Gen};
17use rand::Rng;
18
19impl Arbitrary for Angle {
20    fn arbitrary<G: Gen>(g: &mut G) -> Self {
21        let clockwise = g.gen();
22        let units = g.gen();
23        Angle {
24            clockwise: clockwise,
25            units: if g.gen() { None } else { Some(units) },
26        }
27    }
28}
29
30impl Angle {
31    /// Is this angle zero?
32    ///
33    /// ```
34    /// use integer_angles::Angle;
35    ///
36    /// assert!(Angle::zero().is_zero());
37    /// assert!(!Angle::pi().is_zero());
38    /// ```
39    pub fn is_zero(&self) -> bool {
40        self.units == Some(0)
41    }
42
43    /// Divide pi by some number, and return the result
44    ///
45    /// ```
46    /// use integer_angles::Angle;
47    ///
48    /// assert_eq!(Angle::pi_over(2) * 2, Angle::pi());
49    /// ```
50    pub fn pi_over(n: i64) -> Angle {
51        Angle::pi() / n
52    }
53
54    /// Don't change the direction of the angle, but possibly
55    /// change the direction to get to that angle.
56    ///
57    /// ```
58    /// use integer_angles::Angle;
59    ///
60    /// assert_eq!(Angle::pi_over(2).set_clockwise(true), -3 * Angle::pi_over(2));
61    /// ```
62    pub fn set_clockwise(&self, clockwise: bool) -> Angle {
63        if clockwise && !self.clockwise {
64            *self - Angle::two_pi()
65        } else if !clockwise && self.clockwise {
66            *self + Angle::two_pi()
67        } else {
68            *self
69        }
70    }
71
72    /// Add or subtract an angle depending on the value of the `add` boolean.
73    ///
74    /// ```
75    /// use integer_angles::Angle;
76    ///
77    /// assert_eq!(Angle::pi() + Angle::pi(), Angle::pi().add_or_subtract(true, Angle::pi()));
78    /// assert_eq!(Angle::pi() - Angle::pi(), Angle::pi().add_or_subtract(false, Angle::pi()));
79    /// ```
80    pub fn add_or_subtract(&self, add: bool, other: Angle) -> Angle {
81        if add {
82            *self + other
83        } else {
84            *self - other
85        }
86    }
87
88    /// Create a 0-degree angle
89    ///
90    /// ```
91    /// use integer_angles::Angle;
92    ///
93    /// assert_eq!(Angle::zero() * 2, Angle::zero());
94    /// ```
95    pub fn zero() -> Angle {
96        Angle {
97            clockwise: false,
98            units: Some(0),
99        }
100    }
101
102    /// Determine if the angles are equal while allowing wrapping (meaning, 0 == 2pi).
103    ///
104    /// ```
105    /// use integer_angles::Angle;
106    ///
107    /// assert!(Angle::zero().wrapping_eq(Angle::two_pi()));
108    /// assert!(Angle::pi().wrapping_eq(-Angle::pi()));
109    /// ```
110    pub fn wrapping_eq(&self, other: Angle) -> bool {
111        self.to_i64() == other.to_i64()
112    }
113
114    pub fn wrapping_approx_eq(&self, other: Angle, error: Angle) -> bool {
115        let error = error.to_i64().abs();
116        (self.to_i64() - other.to_i64()) < error
117    }
118
119    /// Get the length of this as an arc given a specific radius
120    ///
121    /// ```
122    /// use integer_angles::Angle;
123    /// use nalgebra::RealField;
124    ///
125    /// assert_eq!(Angle::zero().length(1.0f64), 0.0f64);
126    /// assert_eq!(Angle::two_pi().length(1.0f64), f64::two_pi());
127    /// assert_eq!(Angle::pi_2().length(1.0f64), f64::frac_pi_2());
128    /// ```
129    pub fn length<T: RealField>(&self, radius: T) -> T {
130        if let Some(x) = self.units {
131            radius * convert::<f64, T>(x as f64) / convert::<f64, T>((1u64 << 63) as f64) * T::pi()
132        } else {
133            radius * T::two_pi()
134        }
135    }
136
137    /// Create an angle of pi over two counter-clockwise (for convenience).
138    ///
139    /// ```
140    /// use integer_angles::Angle;
141    ///
142    /// assert_eq!(Angle::pi_2(), Angle::pi_over(2));
143    /// assert_eq!(Angle::pi_2() * 2, Angle::pi());
144    /// ```
145    pub fn pi_2() -> Angle {
146        Angle {
147            clockwise: false,
148            units: Some(1u64 << 62),
149        }
150    }
151
152    /// Create an angle of pi counter-clockwise.
153    ///
154    /// ```
155    /// use integer_angles::Angle;
156    ///
157    /// assert_eq!(Angle::pi() / 2, Angle::pi_2());
158    /// ```
159    pub fn pi() -> Angle {
160        Angle {
161            clockwise: false,
162            units: Some(1u64 << 63),
163        }
164    }
165
166    /// Create an angle that is `two_pi` radians counter-clockwise.
167    ///
168    /// ```
169    /// use integer_angles::Angle;
170    ///
171    /// assert_eq!(Angle::two_pi() * 2, Angle::pi() * 2);
172    /// ```
173    pub fn two_pi() -> Angle {
174        Angle {
175            clockwise: false,
176            units: None,
177        }
178    }
179
180    /// Get the number of units in this angle.  This will be some number between 0 and 2^64 - 1
181    /// where a full circle is 2^64 units.  If this represents a full cirle, `None` will be
182    /// returned.
183    ///
184    /// ```
185    /// use integer_angles::Angle;
186    ///
187    /// assert_eq!(Angle::zero().units(), Some(0));
188    /// assert_eq!(Angle::two_pi().units(), None);
189    /// ```
190    pub fn units(&self) -> Option<u64> {
191        self.units
192    }
193
194    /// Is this angle clockwise or counter-clockwise?
195    ///
196    /// ```
197    /// use integer_angles::Angle;
198    ///
199    /// assert_eq!(Angle::pi().clockwise(), false);
200    /// assert_eq!((-Angle::pi()).clockwise(), true);
201    /// ```
202    pub fn clockwise(&self) -> bool {
203        self.clockwise
204    }
205
206    /// Returns the cosine of (x * 1<<63) / pi with a very high precision
207    ///
208    /// This only works over the range [-pi/2 to pi/2]
209    fn cos_partial<T: RealField>(x: i64) -> T {
210        assert!(x >= -(1 << 62) && x <= (1 << 62));
211
212        let pi = 1i128 << 63;
213
214        // 58 bits per thing
215        let a = 1152921504606846976;
216        let b = -5689439577989151082;
217        let c = 4679376491554475694;
218        let d = -1539453160513334483;
219        let e = 271317744433296946;
220        let f = -29753320046888181;
221        let g = 2224647666372392;
222        let h = -120639134191123;
223        let i = 4959362087252;
224        let j = -155841981042;
225
226        let coeffs = vec![i, h, g, f, e, d, c, b, a];
227
228        let mut sum: i128 = j;
229        let x = x as i128;
230
231        for i in 0..coeffs.len() {
232            sum *= x; // this order of operations is 'blessed' in that it prevents
233            sum /= pi; // overflow on the i128 value, while keeping enough bits of
234            sum *= x; // precision to convert to a float
235            sum /= pi;
236            sum += coeffs[i];
237        }
238
239        // Keep in mind 1<<60 is representable exactly with a float
240        T::from_i64(sum as i64).unwrap() / T::from_i64(1i64 << 60).unwrap()
241    }
242
243    /// Returns the angle as an i64 (for doing math), and wrap 2pi to 0.  If clockwise, return
244    /// the negative of the result.
245    ///
246    /// ```
247    /// use integer_angles::Angle;
248    ///
249    /// assert_eq!(Angle::pi().to_i64(), i64::min_value());
250    /// assert_eq!(Angle::two_pi().to_i64(), 0);
251    /// assert_eq!(-Angle::pi_2().to_i64(), i64::min_value() / 2);
252    /// assert_eq!(Angle::pi_2().to_i64(), -(i64::min_value() / 2));
253    /// ```
254    pub fn to_i64(&self) -> i64 {
255        (self.units.unwrap_or(0) as i64).wrapping_mul(if self.clockwise { -1 } else { 1 })
256    }
257
258    /// Returns the cosine of the angle.
259    ///
260    /// ```
261    /// use integer_angles::Angle;
262    ///
263    /// assert_eq!(Angle::zero().cos::<f64>(), 1.0f64);
264    /// assert_eq!(Angle::pi_2().cos::<f64>(), 0.0f64);
265    /// assert_eq!(Angle::pi().cos::<f64>(), -1.0f64);
266    /// assert_eq!((Angle::pi_2() * 3).cos::<f64>(), 0.0f64);
267    /// assert_eq!(Angle::two_pi().cos::<f64>(), 1.0f64);
268    /// ```
269    pub fn cos<T: RealField>(&self) -> T {
270        let x = self.to_i64();
271        if x < -(1 << 62) {
272            // x is in the range [-pi to pi/2)
273            -Angle::cos_partial::<T>(x.wrapping_add(i64::min_value()))
274        } else if x > (1 << 62) {
275            -Angle::cos_partial::<T>(x.wrapping_add(i64::min_value()))
276        } else {
277            Angle::cos_partial(x)
278        }
279    }
280
281    /// Returns the cosine of the angle.
282    ///
283    /// ```
284    /// use integer_angles::Angle;
285    ///
286    /// assert_eq!(Angle::zero().sin::<f64>(), 0.0f64);
287    /// assert_eq!(Angle::pi_2().sin::<f64>(), 1.0f64);
288    /// assert_eq!(Angle::pi().sin::<f64>(), 0.0f64);
289    /// assert_eq!((3 * Angle::pi_2()).sin::<f64>(), -1.0f64);
290    /// assert_eq!(Angle::two_pi().sin::<f64>(), 0.0f64);
291    /// ```
292    pub fn sin<T: RealField>(&self) -> T {
293        (*self - Angle::pi_2()).cos() // no need to redo the polynomial, as the subtraction of pi_2 is lossless
294    }
295
296    /// Returns the tangent of the angle.
297    ///
298    /// ```
299    /// use integer_angles::Angle;
300    ///
301    /// assert_eq!(Angle::pi_over(4).tan::<f64>(), 1.0f64);
302    /// ```
303    pub fn tan<T: RealField>(&self) -> T {
304        self.sin::<T>() / self.cos::<T>() // here... yeah, I should make a new polynomial
305    }
306
307    /// Compute the arccos of the number.
308    ///
309    /// ```
310    /// use integer_angles::Angle;
311    ///
312    /// assert_eq!(Angle::acos(0.0f64), Angle::pi_over(2));
313    /// ```
314    pub fn acos<T: RealField>(x: T) -> Angle {
315        let mut min = 0;
316        let mut max = 1 << 63;
317
318        let mut error = max - min;
319        while error > 1 {
320            let middle = (max + min) / 2;
321            let cos: T = (Angle {
322                units: Some(middle),
323                clockwise: false,
324            })
325            .cos();
326            if cos == x {
327                return Angle {
328                    units: Some(middle),
329                    clockwise: false,
330                };
331            } else if cos < x {
332                max = middle;
333            } else {
334                min = middle;
335            }
336            error = max - min;
337        }
338
339        Angle {
340            units: Some((max + min) / 2),
341            clockwise: false,
342        }
343    }
344
345    /// Compute the arcsin of the number.
346    ///
347    /// ```
348    /// use integer_angles::Angle;
349    ///
350    /// assert_eq!(Angle::asin(0.5f64).sin::<f64>(), 0.5f64);
351    /// ```
352    pub fn asin<T: RealField>(x: T) -> Angle {
353        let mut min: i64 = -(1 << 62);
354        let mut max: i64 = 1 << 62;
355
356        while min + 1 < max {
357            let middle = (max + min) / 2;
358            let sin: T = (Angle {
359                units: Some(middle as u64),
360                clockwise: false,
361            })
362            .sin();
363            if sin == x {
364                break;
365            } else if sin < x {
366                min = middle;
367            } else {
368                max = middle;
369            }
370        }
371
372        let x = ((max + min) / 2) as u64;
373        if x > (1 << 63) {
374            Angle {
375                units: Some(u64::max_value() - x + 1),
376                clockwise: true,
377            }
378        } else {
379            Angle {
380                units: Some(x),
381                clockwise: false,
382            }
383        }
384    }
385
386    /// Compute the arctan of the number.
387    ///
388    /// ```
389    /// use integer_angles::Angle;
390    ///
391    /// assert_eq!(Angle::atan(1.0f64), Angle::pi_over(4));
392    /// ```
393    pub fn atan<T: RealField>(x: T) -> Angle {
394        let mut min: i64 = -(1 << 62);
395        let mut max: i64 = 1 << 62;
396
397        while min + 1 < max {
398            let middle = (max + min) / 2;
399            let tan: T = (Angle {
400                units: Some(middle as u64),
401                clockwise: false,
402            })
403            .tan();
404            if tan == x {
405                break;
406            } else if tan < x {
407                min = middle;
408            } else {
409                max = middle;
410            }
411        }
412
413        let x = ((max + min) / 2) as u64;
414        if x > (1 << 63) {
415            Angle {
416                units: Some(u64::max_value() - x + 1),
417                clockwise: true,
418            }
419        } else {
420            Angle {
421                units: Some(x),
422                clockwise: false,
423            }
424        }
425    }
426
427    /// Compute the atan(y / x) but keeping the sign of y and x.
428    ///
429    /// Returns None if both y and x are 0.
430    ///
431    /// ```
432    /// use integer_angles::Angle;
433    ///
434    /// assert_eq!(Angle::atan2(1.0f64, 0.0).unwrap(), Angle::pi_2());
435    /// assert_eq!(Angle::atan2(0.0, 0.0f64), None);
436    /// ```
437    pub fn atan2<T: RealField>(y: T, x: T) -> Option<Angle> {
438        if x == T::zero() && y == T::zero() {
439            None // I strongly disagree with IEEE on the correct result here.
440                 // This is clearly the correct result.
441        } else {
442            let r = if x == T::zero() {
443                y.abs()
444            } else if y == T::zero() {
445                x.abs()
446            } else {
447                x.hypot(y)
448            };
449
450            if (r + x).abs() >= y.abs() {
451                // x == 0, y != 0 or x and y both != 0
452                Some(Angle::atan(y / (r + x)) * 2)
453            } else if y.abs() > T::zero() {
454                // x and y both != 0
455                Some(Angle::atan((r - x) / y) * 2)
456            } else if x < T::zero() {
457                // x < 0 && y == 0
458                Some(Angle::pi())
459            } else {
460                // x > 0 && y == 0
461                Some(Angle::pi() * -1)
462            }
463        }
464    }
465
466    /// Convert the angle into radians.  This can lose precision pretty easily
467    ///
468    /// ```
469    /// use integer_angles::Angle;
470    /// use nalgebra::RealField;
471    ///
472    /// assert_eq!(Angle::pi().radians::<f64>(), f64::pi());
473    /// ```
474    pub fn radians<T: RealField>(self) -> T {
475        match (self.units, self.clockwise) {
476            (None, true) => -T::two_pi(),
477            (None, false) => T::two_pi(),
478            (Some(0), _) => T::zero(),
479            (Some(x), clockwise) => {
480                let x: T = convert::<f64, T>(x as f64) / convert::<f64, T>((1u64 << 63) as f64);
481                let x = x * T::pi();
482
483                if !clockwise {
484                    x
485                } else {
486                    -x
487                }
488            }
489        }
490    }
491
492    pub fn degrees<T: RealField>(self) -> T {
493        match (self.units, self.clockwise) {
494            (None, true) => -T::two_pi(),
495            (None, false) => T::two_pi(),
496            (Some(0), _) => T::zero(),
497            (Some(x), clockwise) => {
498                let x: T = convert::<f64, T>(x as f64) / convert::<f64, T>((1u64 << 63) as f64);
499                let x = x * convert::<f64, T>(180.0f64);
500
501                if !clockwise {
502                    x
503                } else {
504                    -x
505                }
506            }
507        }
508    }
509}
510
511impl<T: RealField> From<T> for Angle {
512    /// Convert from radians into an angle.  Panics if NaN or infinity is received
513    ///
514    /// ```
515    /// use integer_angles::Angle;
516    /// use nalgebra::RealField;
517    ///
518    /// assert_eq!(Angle::from(f64::pi()), Angle::pi());
519    /// ```
520    fn from(real: T) -> Angle {
521        // should this be try_from, given NaN and Infinity for floats?
522        let mut real = real;
523        if real < T::zero() {
524            real = -convert::<f64, T>(f64::rem_euclid(-try_convert(real).unwrap(), f64::two_pi()));
525        } else if real > T::zero() {
526            real = convert::<f64, T>(f64::rem_euclid(try_convert(real).unwrap(), f64::two_pi()));
527        }
528
529        if real == T::zero() {
530            Angle {
531                clockwise: false,
532                units: Some(0),
533            }
534        } else if real == T::two_pi() {
535            // This is technically lossy, but I don't care
536            Angle {
537                clockwise: false,
538                units: None,
539            }
540        } else if real == -T::two_pi() {
541            Angle {
542                clockwise: true,
543                units: None,
544            }
545        } else if real > T::zero() {
546            let i =
547                try_convert((real * T::from_u64(Angle::pi().units.unwrap()).unwrap()) / T::pi())
548                    .unwrap();
549            Angle {
550                clockwise: false,
551                units: Some(i as u64),
552            }
553        } else if real < T::zero() {
554            let i =
555                try_convert(((-real) * T::from_u64(Angle::pi().units.unwrap()).unwrap()) / T::pi())
556                    .unwrap();
557            Angle {
558                clockwise: true,
559                units: Some(i as u64),
560            }
561        } else {
562            panic!("NaN or Infinity passed into From<T: RealField> to Angle");
563        }
564    }
565}
566
567#[test]
568fn negative_is_positive() {
569    assert_eq!(Angle::from(-2.0f64).units, Angle::from(2.0f64).units);
570}
571
572impl Mul<i64> for Angle {
573    type Output = Angle;
574
575    fn mul(self, rhs: i64) -> Self {
576        let flip_clockwise = rhs < 0;
577
578        let rhs: u64 = if rhs > 0 {
579            rhs as u64
580        } else if rhs == i64::min_value() {
581            1 << 63
582        } else {
583            rhs.abs() as u64
584        };
585
586        match self.units {
587            None => Angle {
588                units: None,
589                clockwise: self.clockwise ^ flip_clockwise,
590            },
591            Some(x) => {
592                let units = x.wrapping_mul(rhs);
593                if units == 0 && x != 0 && rhs != 0 {
594                    // we wrapped to 0, so make this two pi, instead of 0 radians
595                    Angle {
596                        units: None,
597                        clockwise: self.clockwise ^ flip_clockwise,
598                    }
599                } else {
600                    Angle {
601                        units: Some(units),
602                        clockwise: self.clockwise ^ flip_clockwise,
603                    }
604                }
605            }
606        }
607    }
608}
609
610impl Mul<Angle> for i64 {
611    type Output = Angle;
612
613    fn mul(self, rhs: Angle) -> Angle {
614        rhs * self
615    }
616}
617
618impl MulAssign<i64> for Angle {
619    fn mul_assign(&mut self, rhs: i64) {
620        *self = *self * rhs;
621    }
622}
623
624impl Div<i64> for Angle {
625    type Output = Angle;
626
627    fn div(self, rhs: i64) -> Angle {
628        let invert_clockwise = rhs < 0;
629        let rhs = if rhs > 0 {
630            rhs as u64
631        } else if rhs == i64::min_value() {
632            1u64 << 63
633        } else {
634            (-rhs) as u64
635        };
636
637        match self.units {
638            None => {
639                if rhs == 1 {
640                    Angle {
641                        units: self.units,
642                        clockwise: self.clockwise ^ invert_clockwise,
643                    }
644                } else {
645                    Angle {
646                        units: Some(((1i128 << 64) / (rhs as i128)) as u64),
647                        clockwise: self.clockwise ^ invert_clockwise,
648                    }
649                }
650            }
651            Some(x) => Angle {
652                units: Some(x / rhs),
653                clockwise: self.clockwise ^ invert_clockwise,
654            },
655        }
656    }
657}
658
659impl DivAssign<i64> for Angle {
660    fn div_assign(&mut self, rhs: i64) {
661        *self = *self / rhs;
662    }
663}
664
665fn add(units1: Option<u64>, units2: Option<u64>) -> Option<u64> {
666    match (units1, units2) {
667        (None, None) => None,
668        (None, Some(x)) => Some(x),
669        (Some(x), None) => Some(x),
670        (Some(x), Some(y)) => {
671            if x == 0 && y == 0 {
672                Some(0)
673            } else if x.wrapping_add(y) == 0 {
674                None
675            } else {
676                Some(x.wrapping_add(y))
677            }
678        }
679    }
680}
681
682fn sub(units1: Option<u64>, units2: Option<u64>) -> Option<u64> {
683    match (units1, units2) {
684        (None, None) => Some(0),
685        (None, Some(x)) => {
686            if x == 0 {
687                None
688            } else {
689                Some(u64::max_value().wrapping_sub(x - 1))
690            }
691        }
692        (Some(x), None) => {
693            if x == 0 {
694                None
695            } else {
696                Some(u64::max_value().wrapping_sub(x - 1))
697            }
698        }
699        (Some(x), Some(y)) => {
700            if x > y {
701                Some(x - y)
702            } else {
703                Some(y - x)
704            }
705        }
706    }
707}
708
709impl Add for Angle {
710    type Output = Self;
711
712    fn add(self, other: Self) -> Self {
713        if self.clockwise == other.clockwise {
714            Angle {
715                units: add(self.units, other.units),
716                clockwise: self.clockwise,
717            }
718        } else {
719            // either x - y or -x + y == y - x == -(x - y).  So, the result (x - y) is useful
720            let result = sub(self.units, other.units);
721            let flip_sign = if self.units.is_none() {
722                false
723            } else if other.units.is_none() {
724                true
725            } else {
726                self.units.unwrap() < other.units.unwrap()
727            };
728
729            Angle {
730                units: result,
731                clockwise: self.clockwise ^ flip_sign,
732            }
733        }
734    }
735}
736
737use std::cmp::Ordering;
738
739impl Ord for Angle {
740    fn cmp(&self, other: &Self) -> Ordering {
741        if self == other {
742            Ordering::Equal
743        } else if !self.clockwise && other.clockwise {
744            Ordering::Greater
745        } else if self.clockwise && !other.clockwise {
746            Ordering::Less
747        } else if self.clockwise && other.clockwise {
748            if self.units.is_none() {
749                Ordering::Less
750            } else if other.units.is_none() {
751                Ordering::Greater
752            } else if self.units.unwrap() < other.units.unwrap() {
753                Ordering::Greater
754            } else {
755                Ordering::Less
756            }
757        } else {
758            if self.units.is_none() {
759                Ordering::Greater
760            } else if other.units.is_none() {
761                Ordering::Less
762            } else if self.units.unwrap() < other.units.unwrap() {
763                Ordering::Less
764            } else {
765                Ordering::Greater
766            }
767        }
768    }
769}
770
771impl PartialOrd for Angle {
772    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
773        Some(self.cmp(other))
774    }
775}
776
777impl AddAssign<Angle> for Angle {
778    fn add_assign(&mut self, other: Self) {
779        *self = *self + other;
780    }
781}
782
783impl Sub for Angle {
784    type Output = Self;
785
786    fn sub(self, other: Self) -> Self {
787        let to_add = Angle {
788            units: other.units,
789            clockwise: !other.clockwise,
790        };
791
792        self + to_add
793    }
794}
795
796impl SubAssign<Angle> for Angle {
797    fn sub_assign(&mut self, other: Self) {
798        *self = *self - other;
799    }
800}
801
802impl Neg for Angle {
803    type Output = Angle;
804
805    fn neg(self) -> Self::Output {
806        Angle {
807            units: self.units,
808            clockwise: !self.clockwise,
809        }
810    }
811}
integer_angles/angle.rs

integer_angles/
angle.rs
