coordinates/three_dimensional/
spherical.rs

1use std::{
2    fmt::Display,
3    ops::{Add, Div, Mul, Neg, Sub},
4};
5
6use num_traits::Float;
7
8use super::{cylindrical::Cylindrical, vector3::Vector3};
9use crate::traits::{Magnitude, Positional, TrigConsts};
10
11#[cfg(feature = "serde")]
12use serde::{Deserialize, Serialize};
13
14/*********************
15 * STRUCT DEFINITION *
16 *********************/
17
18#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
19#[allow(clippy::derived_hash_with_manual_eq)]
20#[derive(Debug, Copy, Clone, Hash)]
21/// A point in 3D space using spherical coordinates as defined by [ISO 80000-2:2019](https://en.wikipedia.org/wiki/Spherical_coordinate_system#Definition).
22///
23/// This means that the coordinates are provided in the order `radius` (`r`), `polar angle` (`theta`), and finally `azimuthal angle` (`phi`)
24/// ## Examples
25///
26/// ```
27/// # use coordinates::three_dimensional::Spherical;
28/// # use crate::coordinates::three_dimensional::ThreeDimensionalConsts;
29/// let right = Spherical::<f64>::new(1.0, 0.0, 0.0);
30/// assert_eq!(right, Spherical::<f64>::UP);
31/// ```
32pub struct Spherical<T: Float> {
33    /// Distance from the origin
34    #[cfg_attr(feature = "serde", serde(rename = "r"))]
35    pub radius: T,
36    /// Angle from the positive `z` direction
37    #[cfg_attr(feature = "serde", serde(rename = "theta"))]
38    pub polar_angle: T,
39    /// angle from the positive `x` direction along the `xy` plane
40    #[cfg_attr(feature = "serde", serde(rename = "phi"))]
41    pub azimuthal_angle: T,
42}
43
44impl<T: Float + TrigConsts> Spherical<T> {
45    /// Maps parameters into appropriate domains.
46    ///
47    /// # Mapping
48    ///
49    /// - `azimuthal angle` ∈ [0,τ)
50    ///     - Doesn't have side effects
51    /// - `Polar angle ∈ [0,π]`
52    ///     - Mutates azimuthal angle
53    /// - `radius ∈ [0,∞)`
54    ///     - Mutates polar angle and azimuthal angle
55    ///
56    /// # Examples
57    /// ## Clamping Azimuthal angle
58    ///
59    /// ```
60    /// # use coordinates::three_dimensional::Spherical;
61    /// # use coordinates::traits::Positional;
62    /// let right = Spherical::<f64>::new(1.0, std::f64::consts::FRAC_PI_2, 0.0);
63    /// let also_right = Spherical::<f64>::new(1.0, std::f64::consts::FRAC_PI_2, std::f64::consts::TAU);
64    ///
65    /// assert!(right.angle_to(&also_right) < std::f64::EPSILON);
66    /// ```
67    /// ## Clamping Polar Angle
68    /// ```
69    /// # use coordinates::three_dimensional::Spherical;
70    /// # use coordinates::traits::Positional;
71    /// let up = Spherical::<f64>::new(1.0, 0.0, 0.0);
72    /// let also_up = Spherical::<f64>::new(1.0, std::f64::consts::TAU, 0.0);
73    /// assert!(up.angle_to(&also_up) < std::f64::EPSILON);
74    /// ```
75    /// ## Clamping Radius
76    /// ```
77    /// # use coordinates::three_dimensional::Spherical;
78    /// # use coordinates::traits::Positional;
79    /// let left = Spherical::<f64>::new(1.0, std::f64::consts::FRAC_PI_2, std::f64::consts::PI);
80    /// let also_left = Spherical::<f64>::new(-1.0, std::f64::consts::FRAC_PI_2, 0.0);
81    ///
82    /// assert!(left.angle_to(&also_left) < std::f64::EPSILON);
83    /// ```
84    //ALTERNATE NEW METHOD
85    pub fn new(radius: T, polar_angle: T, azimuthal_angle: T) -> Spherical<T> {
86        let mut result = Self {
87            radius,
88            polar_angle,
89            azimuthal_angle,
90        };
91        result.set_azimuthal_angle(azimuthal_angle);
92        result.set_polar_angle(polar_angle);
93        result.set_radius(radius);
94
95        result
96    }
97
98    /// Returns the latitude/elevation of the point.
99    ///
100    /// i.e. the polar angle with respect to the equator instead of the
101    /// north pole
102    pub fn get_elevation(&self) -> T {
103        T::FRAC_PI_2 - self.polar_angle
104    }
105
106    /// Ensures the azimuth is always in the range [0,tau)
107    pub fn set_azimuthal_angle(&mut self, azimuthal_angle: T) {
108        if azimuthal_angle.is_sign_positive() {
109            self.azimuthal_angle = azimuthal_angle % T::TAU;
110        } else {
111            self.azimuthal_angle = azimuthal_angle % T::TAU + T::TAU;
112        }
113    }
114
115    /// Ensures polar angle is always in the range [0,pi)
116    pub fn set_polar_angle(&mut self, polar_angle: T) {
117        // `Checked polar angle` ∈ [0,tau) when `polar angle` >= 0
118        // `Checked polar angle` ∈ (0,tau] when `polar angle` <= -0
119        let mut checked_polar_angle = polar_angle % T::TAU
120            + if polar_angle.is_sign_negative() {
121                // If polar angle is negative, checked polar angle ∈ (-tau, 0]
122                // Add tau to move to the range (0,tau]
123                T::TAU
124            } else {
125                T::ZERO
126            };
127
128        if checked_polar_angle > T::PI {
129            // Checked polar angle is out of range, so azimuthal angle
130            // will need to be adjusted as well
131            self.set_azimuthal_angle(self.azimuthal_angle + T::PI);
132            checked_polar_angle = T::TAU - checked_polar_angle;
133        }
134
135        self.polar_angle = checked_polar_angle;
136    }
137
138    /// Ensures radius is always in the range [0,+infinity]
139    pub fn set_radius(&mut self, radius: T) {
140        if radius.is_sign_negative() {
141            self.set_azimuthal_angle(self.azimuthal_angle - T::PI);
142            self.set_polar_angle(T::PI - self.polar_angle);
143            self.radius = -radius;
144        } else {
145            self.radius = radius;
146        }
147    }
148}
149
150impl<T: Float + TrigConsts> super::ThreeDimensionalConsts<T> for Spherical<T> {
151    const ORIGIN: Self = Spherical {
152        azimuthal_angle: T::ZERO,
153        radius: T::ZERO,
154        polar_angle: T::ZERO,
155    };
156
157    const UP: Self = Spherical {
158        azimuthal_angle: T::ZERO,
159        radius: T::ONE,
160        polar_angle: T::ZERO,
161    };
162
163    const DOWN: Self = Spherical {
164        azimuthal_angle: T::ZERO,
165        radius: T::ONE,
166        polar_angle: T::PI,
167    };
168
169    const FORWARD: Self = Spherical {
170        azimuthal_angle: T::FRAC_PI_2,
171        radius: T::ONE,
172        polar_angle: T::FRAC_PI_2,
173    };
174
175    const BACK: Self = Spherical {
176        azimuthal_angle: T::FRAC_3PI_2,
177        radius: T::ONE,
178        polar_angle: T::FRAC_PI_2,
179    };
180
181    const LEFT: Self = Spherical {
182        azimuthal_angle: T::PI,
183        radius: T::ONE,
184        polar_angle: T::FRAC_PI_2,
185    };
186
187    const RIGHT: Self = Spherical {
188        azimuthal_angle: T::ZERO,
189        radius: T::ONE,
190        polar_angle: T::FRAC_PI_2,
191    };
192}
193
194impl<T: Float> crate::traits::Magnitude<T> for Spherical<T> {
195    #[inline]
196    fn magnitude(&self) -> T {
197        self.quick_magnitude()
198    }
199
200    #[inline]
201    fn quick_magnitude(&self) -> T {
202        self.radius
203    }
204}
205
206impl<T: Float> crate::traits::Dot<T> for Spherical<T> {
207    fn dot(&self, other: &Self) -> T {
208        Into::<Vector3<T>>::into(self).dot(&other.into())
209    }
210}
211
212impl<T: Float> crate::traits::Cross3D for Spherical<T> {
213    fn cross(&self, other: &Self) -> Self {
214        Into::<Vector3<T>>::into(self).cross(&other.into()).into()
215    }
216}
217
218impl<T: Float + TrigConsts> Positional<T> for Spherical<T> {
219    /// Using the spherical law of cosines
220    fn angle_to(&self, other: &Self) -> T {
221        let (lat_sin, lat_cos) = self.polar_angle.sin_cos();
222        let (lat_sin_b, lat_cos_b) = other.polar_angle.sin_cos();
223
224        (lat_cos * lat_cos_b
225            + lat_sin * lat_sin_b * (self.azimuthal_angle - other.azimuthal_angle).cos())
226        .acos()
227    }
228}
229
230impl<T: Float + TrigConsts> Neg for Spherical<T> {
231    type Output = Spherical<T>;
232
233    fn neg(self) -> Self::Output {
234        Spherical {
235            polar_angle: T::PI - self.polar_angle,
236            azimuthal_angle: (self.azimuthal_angle + T::PI) % T::TAU,
237            radius: self.radius,
238        }
239    }
240}
241
242impl<T: Float> Add for Spherical<T> {
243    type Output = Spherical<T>;
244
245    fn add(self, rhs: Self) -> Self::Output {
246        (Into::<Vector3<T>>::into(self) + Into::<Vector3<T>>::into(rhs)).into()
247    }
248}
249
250impl<T: Float> Sub for Spherical<T> {
251    type Output = Spherical<T>;
252
253    fn sub(self, rhs: Self) -> Self::Output {
254        (Into::<Vector3<T>>::into(self) - Into::<Vector3<T>>::into(rhs)).into()
255    }
256}
257
258impl<T: Float> Mul<T> for Spherical<T> {
259    type Output = Self;
260
261    fn mul(self, rhs: T) -> Self::Output {
262        Self {
263            radius: self.radius * rhs,
264            azimuthal_angle: self.azimuthal_angle,
265            polar_angle: self.polar_angle,
266        }
267    }
268}
269
270impl<T: Float> Div<T> for Spherical<T> {
271    type Output = Self;
272
273    fn div(self, rhs: T) -> Self::Output {
274        Self {
275            radius: self.radius / rhs,
276            azimuthal_angle: self.azimuthal_angle,
277            polar_angle: self.polar_angle,
278        }
279    }
280}
281
282impl<T: Float + TrigConsts> PartialEq for Spherical<T> {
283    fn eq(&self, other: &Self) -> bool {
284        self.angle_to(other) < T::epsilon()
285    }
286}
287
288impl<T: Float + TrigConsts> Eq for Spherical<T> {}
289impl<T: Float + TrigConsts> PartialOrd for Spherical<T> {
290    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
291        match self.radius.partial_cmp(&other.radius) {
292            Some(core::cmp::Ordering::Equal) => {}
293            ord => return ord,
294        }
295
296        self.polar_angle.partial_cmp(&other.polar_angle)
297    }
298}
299
300impl<T: Float> From<Vector3<T>> for Spherical<T> {
301    fn from(cart: Vector3<T>) -> Self {
302        From::from(&cart)
303    }
304}
305
306impl<T: Float> From<&Vector3<T>> for Spherical<T> {
307    fn from(cart: &Vector3<T>) -> Self {
308        let radius = cart.magnitude();
309        Spherical {
310            polar_angle: (cart.z / radius).acos(),
311            azimuthal_angle: cart.y.atan2(cart.x),
312            radius,
313        }
314    }
315}
316
317impl<T: Float> From<Cylindrical<T>> for Spherical<T> {
318    fn from(cyl: Cylindrical<T>) -> Self {
319        From::<&Cylindrical<T>>::from(&cyl)
320    }
321}
322
323impl<T: Float> From<&Cylindrical<T>> for Spherical<T> {
324    fn from(cyl: &Cylindrical<T>) -> Self {
325        Spherical {
326            radius: cyl.magnitude(),
327            polar_angle: cyl.radius.atan2(cyl.height),
328            azimuthal_angle: cyl.azimuth,
329        }
330    }
331}
332
333impl<T: Float> From<(T, T, T)> for Spherical<T> {
334    fn from(tuple: (T, T, T)) -> Self {
335        Spherical {
336            radius: tuple.0,
337            polar_angle: tuple.1,
338            azimuthal_angle: tuple.2,
339        }
340    }
341}
342
343impl<T: Float> From<Spherical<T>> for (T, T, T) {
344    fn from(sph: Spherical<T>) -> Self {
345        (sph.radius, sph.polar_angle, sph.azimuthal_angle)
346    }
347}
348
349impl<T: Display + Float> Display for Spherical<T> {
350    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
351        write!(
352            f,
353            "({},{},{})",
354            self.radius, self.polar_angle, self.azimuthal_angle
355        )
356    }
357}
358
359#[cfg(test)]
360mod tests {
361    use crate::three_dimensional::ThreeDimensionalConsts;
362    use crate::traits::Positional;
363    use crate::traits::TrigConsts;
364
365    use super::Spherical;
366    use super::Vector3;
367
368    use assert_float_eq::*;
369
370    const ARC_SECOND: f32 = 4.848e-6;
371
372    #[test]
373    pub fn convert_to_cartesian() {
374        let sphericals = [
375            Spherical::<f32>::UP,
376            Spherical::<f32>::DOWN,
377            Spherical::<f32>::BACK,
378            Spherical::<f32>::FORWARD,
379            Spherical::<f32>::LEFT,
380            Spherical::<f32>::RIGHT,
381        ];
382        let cartesians = [
383            Vector3::<f32>::UP,
384            Vector3::<f32>::DOWN,
385            Vector3::<f32>::BACK,
386            Vector3::<f32>::FORWARD,
387            Vector3::<f32>::LEFT,
388            Vector3::<f32>::RIGHT,
389        ];
390
391        for (s, c) in sphericals.into_iter().zip(cartesians.into_iter()) {
392            println!("{s:?}");
393            let s_star: Vector3<f32> = (&s).into();
394            println!("{s_star:?}");
395
396            assert_float_absolute_eq!(
397                c.x,
398                s_star.x,
399                f32::EPSILON * s.azimuthal_angle.abs().log(2.0).max(1.0)
400            );
401            assert_float_absolute_eq!(
402                c.y,
403                s_star.y,
404                f32::EPSILON * s.azimuthal_angle.abs().log(2.0).max(1.0)
405            );
406            assert_float_absolute_eq!(
407                c.z,
408                s_star.z,
409                f32::EPSILON * s.polar_angle.abs().log(2.0).max(1.0)
410            );
411        }
412    }
413
414    #[test]
415    pub fn is_positional() {
416        let up = Spherical::<f32>::UP;
417
418        for point in [
419            Spherical::<f32>::BACK,
420            Spherical::<f32>::FORWARD,
421            Spherical::<f32>::LEFT,
422            Spherical::<f32>::RIGHT,
423        ] {
424            assert_float_relative_eq!(f32::FRAC_PI_2, up.angle_to(&point), f32::EPSILON);
425        }
426
427        assert_float_relative_eq!(f32::PI, up.angle_to(&Spherical::<f32>::DOWN), f32::EPSILON);
428    }
429
430    #[test]
431    pub fn is_positional_over_small_distances() {
432        let roots = [
433            Spherical::<f32>::BACK,
434            Spherical::<f32>::FORWARD,
435            Spherical::<f32>::LEFT,
436            Spherical::<f32>::RIGHT,
437        ];
438
439        for root in roots {
440            small_distance_loop(root);
441        }
442    }
443
444    #[allow(clippy::cast_precision_loss)]
445    fn small_distance_loop(root: Spherical<f32>) {
446        for delta in (-128..128).map(|x| x as f32 / 128.0) {
447            let azi_altered = Spherical::new(1.0, root.polar_angle, root.azimuthal_angle + delta);
448            let polar_altered = Spherical::new(1.0, root.polar_angle + delta, root.azimuthal_angle);
449
450            let delta_azimuth = azi_altered.angle_to(&root);
451            let delta_polar = polar_altered.angle_to(&root);
452
453            println!("Delta={delta}");
454            println!("dist({root}, {azi_altered}) = {delta_azimuth}");
455            assert_float_absolute_eq!(delta_azimuth, delta.abs(), ARC_SECOND / 4.0);
456            println!("dist({root}, {polar_altered}) = {delta_polar}");
457            assert_float_absolute_eq!(delta_polar, delta.abs(), ARC_SECOND / 4.0);
458            println!();
459        }
460    }
461
462    #[allow(clippy::cast_precision_loss)]
463    #[test]
464    pub fn constructor_tests() {
465        type Base = Spherical<f32>;
466        // Test positive rotation around the z axis
467        let mut expected = [Base::RIGHT, Base::FORWARD, Base::LEFT, Base::BACK];
468        println!("Subtest 1");
469        test_constructor(
470            &mut (0..100).map(|i| Base::new(1.0, f32::FRAC_PI_2, i as f32 * f32::FRAC_PI_2)),
471            &expected,
472        );
473
474        // Test negative rotation around the z axis
475        expected.reverse();
476        expected.rotate_right(1);
477        println!("Subtest 2");
478        test_constructor(
479            &mut (0..100).map(|i| Base::new(1.0, f32::FRAC_PI_2, i as f32 * -f32::FRAC_PI_2)),
480            &expected,
481        );
482
483        // Test positive rotation around the y axis for points starting at (1. 0, pi/2)
484        expected = [Base::UP, Base::RIGHT, Base::DOWN, Base::LEFT];
485        println!("Subtest 3");
486        test_constructor(
487            &mut (0..100).map(|i| Base::new(1.0, i as f32 * f32::FRAC_PI_2, 0.0)),
488            &expected,
489        );
490
491        // Test negative rotation around the y axis for points starting at (1. 0, pi/2)
492        expected.reverse();
493        expected.rotate_right(1);
494        println!("Subtest 4");
495        test_constructor(
496            &mut (0..100).map(|i| Base::new(1.0, i as f32 * -f32::FRAC_PI_2, 0.0)),
497            &expected,
498        );
499
500        // Test positive rotation around the x axis for points starting at (1. 0, pi/2)
501        expected = [Base::UP, Base::FORWARD, Base::DOWN, Base::BACK];
502        println!("Subtest 5");
503        test_constructor(
504            &mut (0..100).map(|i| Base::new(1.0, i as f32 * f32::FRAC_PI_2, f32::FRAC_PI_2)),
505            &expected,
506        );
507
508        // Test negative rotation around the x axis for points starting at (1. 0, pi/2)
509        expected.reverse();
510        expected.rotate_right(1);
511        println!("Subtest 6");
512        test_constructor(
513            &mut (0..100).map(|i| Base::new(1.0, i as f32 * -f32::FRAC_PI_2, f32::FRAC_PI_2)),
514            &expected,
515        );
516    }
517
518    #[allow(clippy::cast_precision_loss)]
519    fn test_constructor(
520        iterator: &mut dyn Iterator<Item = Spherical<f32>>,
521        expected: &[Spherical<f32>],
522    ) {
523        for (total_i, entry) in iterator.enumerate() {
524            // Set i between 0 and expected.len() - 1
525            let i = total_i % expected.len();
526
527            // assert_float_absolute_eq!(
528            //     entry.azimuthal_angle,
529            //     expected[i].azimuthal_angle,
530            //     f32::EPSILON * (total_i as f32 * f32::FRAC_PI_2).log(2.0).max(1.0)
531            // );
532            // assert_float_absolute_eq!(
533            //     entry.polar_angle,
534            //     expected[i].polar_angle,
535            //     f32::EPSILON * (total_i as f32 * f32::FRAC_PI_2).log(2.0).max(1.0)
536            // );
537            let deviation = entry.angle_to(&expected[i]);
538
539            print!("Winding of {:5.2}τ rad ", (total_i as f32) / 4.0);
540            // assert_float_relative_eq!(deviation, 0.0, f32::EPSILON);
541            // Maximum acceptable inaccuracy is 0.25" (seconds of an arc)
542            if deviation.abs() > ARC_SECOND {
543                println!("failed (deviation {})", deviation.abs());
544                println!("Expected {}, got {}", expected[i], entry);
545            }
546            assert_float_relative_eq!(deviation, 0.0, ARC_SECOND / 4.0);
547
548            println!("worked (deviations: {deviation})");
549        }
550        println!("\x1b[32mSubtest Passed\x1b[0m");
551        println!();
552    }
553}
coordinates/three_dimensional/spherical.rs

coordinates/three_dimensional/
spherical.rs
