rkepler 0.4.2

/*
Copyright (c) 2022 Peter Hristozov / Петър Христозов

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

//! Spherical coordinates.

/// Represent unit vector in spherical coordinates
pub mod sph {

    use crate::math::angle::*;
    use crate::math::vector::vec_p;

    /// Unit vector in spherical coordinates
    #[derive(Debug)]
    pub struct Sph {
        /// longitude angle in radians
        pub lon: f64,
        /// latitude angle in radians
        pub lat: f64,
    }
    impl Sph {
        /// Convert unit vector from spherical to Cartesian coordinates
        /// # Arguments
        /// # Returns
        /// Unit vector in Cartesian coordinate or all so direction cosines
        pub fn to_cart(&self) -> vec_p::VecP {
            let (slon, clon) = &self.lon.sin_cos();
            let (slat, clat) = &self.lat.sin_cos();
            vec_p::VecP {
                x: clon * clat,
                y: slon * clat,
                z: *slat,
            }
        }
    }

    /// Convert position vector from Cartesian coordinates to unit vector in spherical coordinates
    /// # Arguments
    /// * `p` position vector in Cartesian coordinates
    /// # Returns
    /// unit vector in spherical coordinates
    pub fn from_cart(p: &vec_p::VecP) -> Sph {
        let d = p.x * p.x + p.y * p.y;
        Sph {
            lon: if d != 0.0 {
                norm_dblpi(p.y.atan2(p.x))
            } else {
                0.0
            },
            lat: if p.z != 0.0 { p.z.atan2(d.sqrt()) } else { 0.0 },
        }
    }

    /// Angular separation between two sets of spherical coordinates.
    /// # Arguments
    /// * `a` first point in spherical coordinates
    /// * `b` second point in spherical coordinates
    /// # Returns
    /// Angular separation in radians
    pub fn sep_angl(a: &Sph, b: &Sph) -> f64 {
        vec_p::sep_angl(&a.to_cart(), &b.to_cart())
    }

    /// Position-angle from spherical coordinates.
    /// # Arguments
    /// * `a` first point in spherical coordinates
    /// * `b` second point in spherical coordinates
    /// # Returns
    /// Position angle of `b` with respect to `a` in radians, range -pi to +pi, east +pi/2
    pub fn pos_angl(a: &Sph, b: &Sph) -> f64 {
        let dl = b.lon - a.lon;
        let y = dl.sin() * b.lat.cos();
        let x = b.lat.sin() * a.lat.cos() - b.lat.cos() * a.lat.sin() * dl.cos();
        if (x != 0.0) || (y != 0.0) {
            y.atan2(x)
        } else {
            0.0
        }
    }
}

/// Represent position vector in spherical coordinates
pub mod sph_p {

    use crate::math::angle::*;
    use crate::math::vector::vec_p;

    /// Vector in spherical coordinates
    #[derive(Debug)]
    pub struct SphP {
        /// longitude angle in radians
        pub lon: f64,
        /// latitude angle in radians
        pub lat: f64,
        /// radial distance
        pub rad: f64,
    }
    impl SphP {
        /// Convert position vector from spherical to Cartesian coordinates
        /// # Arguments
        /// # Returns
        /// Position vector in Cartesian coordinates.
        pub fn to_cart(&self) -> vec_p::VecP {
            let (slon, clon) = &self.lon.sin_cos();
            let (slat, clat) = &self.lat.sin_cos();
            vec_p::VecP {
                x: &self.rad * clon * clat,
                y: &self.rad * slon * clat,
                z: &self.rad * slat,
            }
        }
    }

    /// Convert position vector from Cartesian to spherical coordinates
    /// # Arguments
    /// * `p` position vector in Cartesian coordinates
    /// # Returns
    /// position vector in spherical coordinates
    pub fn from_cart(p: &vec_p::VecP) -> SphP {
        let d2 = p.x * p.x + p.y * p.y;
        SphP {
            lon: if d2 != 0.0 {
                norm_dblpi(p.y.atan2(p.x))
            } else {
                0.0
            },
            lat: if p.z != 0.0 {
                p.z.atan2(d2.sqrt())
            } else {
                0.0
            },
            rad: p.modul(),
        }
    }

    /// Angular separation between two vectors in spherical coordinates.
    /// # Arguments
    /// * `a` first vector (not necessarily unit length)
    /// * `b` second vector (not necessarily unit length)
    /// # Returns
    /// Angular separation in radians, always positive
    pub fn sep_angl(a: &SphP, b: &SphP) -> f64 {
        super::sph::sep_angl(
            &super::sph::Sph {
                lon: a.lon,
                lat: a.lat,
            },
            &super::sph::Sph {
                lon: b.lon,
                lat: b.lat,
            },
        )
    }

    /// Position-angle from two vectors in spherical coordinates.
    /// # Arguments
    /// * `a` first vector in spherical coordinates
    /// * `b` second vector in spherical coordinates
    /// # Returns
    /// Position angle of `b` with respect to `a` in radians, range -pi to +pi, east +pi/2
    pub fn pos_angl(a: &SphP, b: &SphP) -> f64 {
        super::sph::pos_angl(
            &super::sph::Sph {
                lon: a.lon,
                lat: a.lat,
            },
            &super::sph::Sph {
                lon: b.lon,
                lat: b.lat,
            },
        )
    }
}

/// Represent position and velocity vectors in spherical coordinates
pub mod sph_pv {

    use crate::math::angle::*;
    use crate::math::scalar::scl_sv::SclSv;
    use crate::math::vector::vec_pv;

    /// Position and velocity vectors in spherical coordinates
    #[derive(Debug)]
    pub struct SphPv {
        /// longitude angle in radians
        pub lon: f64,
        /// latitude angle in radians
        pub lat: f64,
        /// radial distance
        pub rad: f64,
        /// velocity in longitude
        pub vlon: f64,
        /// velocity in latitude
        pub vlat: f64,
        /// radial velocity
        pub vrad: f64,
    }
    impl SphPv {
        pub fn to_cart(&self) -> vec_pv::VecPv {
            let (slon, clon) = &self.lon.sin_cos();
            let (slat, clat) = &self.lat.sin_cos();
            vec_pv::VecPv {
                x: (&self.rad * clon * clat),
                y: (&self.rad * slon * clat),
                z: (&self.rad * slat),
                vx: (clat * clon * &self.vrad
                    - &self.rad * clat * slon * &self.vlon
                    - &self.rad * slat * clon * &self.vlat),
                vy: (clat * slon * &self.vrad + &self.rad * clat * clon * &self.vlon
                    - &self.rad * slat * slon * &self.vlat),
                vz: (slat * &self.vrad + &self.rad * clat * &self.vlat),
            }
        }
    }

    pub fn from_cart(p: &vec_pv::VecPv) -> SphPv {
        let d2 = p.x * p.x + p.y * p.y;
        let d = d2.sqrt();
        let vd = (p.x * p.vx + p.y * p.vy) / d;
        let r = p.modul();
        let r2 = r.s * r.s;
        if d2 != 0.0 {
            SphPv {
                lon: norm_dblpi(p.y.atan2(p.x)),
                lat: if p.z != 0.0 { p.z.atan2(d) } else { 0.0 },
                rad: r.s,
                vlon: (p.x * p.vy - p.y * p.vx) / d2,
                vlat: (d * p.vz - p.z * vd) / r2,
                vrad: r.v,
            }
        } else {
            SphPv {
                lon: (0.0),
                lat: (if p.z != 0.0 { p.z.atan2(d) } else { 0.0 }),
                rad: (r.s),
                vlon: (0.0),
                vlat: (0.0),
                vrad: (r.v),
            }
        }
    }

    pub fn sep_angl(a: &SphPv, b: &SphPv) -> SclSv {
        let carta = a.to_cart();
        let cartb = b.to_cart();
        vec_pv::sep_angl(&carta, &cartb)
    }

    pub fn update(t: f64, t0: f64, sph0: &SphPv) -> SphPv {
        let dt = t - t0;
        SphPv {
            lon: (sph0.lon + dt * sph0.vlon),
            lat: (sph0.lat + dt * sph0.vlat),
            rad: (sph0.rad + dt * sph0.vrad),
            vlon: (sph0.vlon),
            vlat: (sph0.vlat),
            vrad: (sph0.vrad),
        }
    }

    pub fn interp(t: f64, t0: f64, sph0: &SphPv, t1: f64, sph1: &SphPv) -> SphPv {
        let dt = t - t0;
        let dta = 1.0 / (t1 - t0);
        let acc_lon = (sph1.vlon - sph0.vlon) * dta;
        let acc_lat = (sph1.vlat - sph0.vlat) * dta;
        let acc_rad = (sph1.vrad - sph0.vrad) * dta;
        SphPv {
            lon: (sph0.lon + dt * (sph0.vlon + 0.5 * dt * acc_lon)),
            lat: (sph0.lat + dt * (sph0.vlat + 0.5 * dt * acc_lat)),
            rad: (sph0.rad + dt * (sph0.vrad + 0.5 * dt * acc_rad)),
            vlon: (sph0.vlon + dt * acc_lon),
            vlat: (sph0.vlat + dt * acc_lat),
            vrad: (sph0.vrad + dt * acc_rad),
        }
    }
}

/// Represent position, velocity and acceleration vectors in spherical coordinates
pub mod sph_pva {

    use crate::math::angle::*;
    use crate::math::scalar::scl_sva::SclSva;
    use crate::math::vector::vec_pva;

    /// Position, velocity and acceleration vectors in spherical coordinates
    #[derive(Debug)]
    pub struct SphPva {
        /// longitude angle in radians
        pub lon: f64,
        /// latitude angle in radians
        pub lat: f64,
        /// radial distance
        pub rad: f64,
        /// velocity in longitude
        pub vlon: f64,
        /// velocity in latitude
        pub vlat: f64,
        /// radial velocity
        pub vrad: f64,
        /// acceleration in longitude
        pub alon: f64,
        /// acceleration in latitude
        pub alat: f64,
        /// radial acceleration
        pub arad: f64,
    }
    impl SphPva {
        pub fn to_cart(&self) -> vec_pva::VecPva {
            let (slon, clon) = self.lon.sin_cos();
            let (slat, clat) = self.lat.sin_cos();
            vec_pva::VecPva {
                x: (self.rad * clon * clat),
                y: (self.rad * slon * clat),
                z: (self.rad * slat),
                vx: (clat * clon * self.vrad
                    - self.rad * clat * slon * self.vlon
                    - self.rad * slat * clon * self.vlat),
                vy: (clat * slon * self.vrad + self.rad * clat * clon * self.vlon
                    - self.rad * slat * slon * self.vlat),
                vz: (slat * self.vrad + self.rad * clat * self.vlat),
                ax: (clat * clon * self.arad
                    - 2.0 * clat * slon * self.vlon * self.vrad
                    - 2.0 * slat * clon * self.vlat * self.vrad
                    - self.rad * clat * slon * self.alon
                    - self.rad * clat * clon * self.vlon * self.vlon
                    + 2.0 * self.rad * slat * slon * self.vlat * self.vlon
                    - self.rad * slat * clon * self.alat
                    - self.rad * clat * clon * self.vlat * self.vlat), //TODO
                ay: (clat * slon * self.arad + 2.0 * clat * clon * self.vlon * self.vrad
                    - 2.0 * slat * slon * self.vlat * self.vrad
                    + self.rad * clat * clon * self.alon
                    - self.rad * clat * slon * self.vlon * self.vlon
                    - 2.0 * self.rad * slat * clon * self.vlat * self.vlon
                    - self.rad * slat * slon * self.alat
                    - self.rad * clat * slon * self.vlat * self.vlat),
                az: (slat * self.arad
                    + 2.0 * clat * self.vlat * self.vrad
                    + self.rad * clat * self.alat
                    - self.rad * slat * self.alat * self.alat),
            }
        }
    }

    pub fn from_cart(p: &vec_pva::VecPva) -> SphPva {
        let d2 = p.x * p.x + p.y * p.y;
        let d = d2.sqrt();
        let vd = (p.x * p.vx + p.y * p.vy) / d;
        let ad = (p.x * p.ax + p.vx * p.vx + p.y * p.ay + p.vy * p.vy - vd * vd) / d;
        let zd = 2.0 * (p.z * p.vz + d * vd);
        let r = p.modul();
        let r2 = r.s * r.s;
        if d2 != 0.0 {
            SphPva {
                lon: (norm_dblpi(p.y.atan2(p.x))),
                lat: (if p.z != 0.0 { p.z.atan2(d) } else { 0.0 }),
                rad: (r.s),
                vlon: ((p.x * p.vy - p.y * p.vx) / d2),
                vlat: ((d * p.vz - p.z * vd) / r2),
                vrad: (r.v),
                alon: (p.x * p.ay - p.y * p.ax) / d2
                    + 2.0 * vd * d * (-p.x * p.vy + p.y * p.vx) / (d2 * d2), //TODO
                alat: (d * p.az - p.z * ad) / r2 + zd * (-d * p.vz + p.z * vd) / (r2 * r2), //TODO
                arad: (r.a),
            }
        } else {
            SphPva {
                lon: (0.0),
                lat: (if p.z != 0.0 { p.z.atan2(d) } else { 0.0 }),
                rad: (r.s),
                vlon: (0.0),
                vlat: (0.0),
                vrad: (r.v),
                alon: (0.0),
                alat: (0.0),
                arad: (r.a),
            }
        }
    }

    pub fn sep_angl(a: &SphPva, b: &SphPva) -> SclSva {
        vec_pva::sep_angl(&a.to_cart(), &b.to_cart())
    }

    pub fn update(t: f64, t0: f64, sph0: &SphPva) -> SphPva {
        let dt = t - t0;
        SphPva {
            lon: (sph0.lon + dt * (sph0.vlon + 0.5 * dt * sph0.alon)),
            lat: (sph0.lat + dt * (sph0.vlat + 0.5 * dt * sph0.alat)),
            rad: (sph0.rad + dt * (sph0.vrad + 0.5 * dt * sph0.arad)),
            vlon: (sph0.vlon + dt * sph0.alon),
            vlat: (sph0.vlat + dt * sph0.alat),
            vrad: (sph0.vrad + dt * sph0.arad),
            alon: (sph0.alon),
            alat: (sph0.alat),
            arad: (sph0.arad),
        }
    }

    pub fn interp(t: f64, t0: f64, sph0: &SphPva, t1: f64, sph1: &SphPva) -> SphPva {
        let dt = t - t0;
        let dta = 1.0 / t1 - t0;
        let acc_lon = (sph1.alon - sph0.alon) * dta;
        let acc_lat = (sph1.alat - sph0.alat) * dta;
        let acc_rad = (sph1.arad - sph0.arad) * dta;
        SphPva {
            lon: (sph0.lon + dt * (sph0.vlon + dt * (0.5 * sph0.alon + dt * acc_lon / 6.0))),
            lat: (sph0.lat + dt * (sph0.vlat + dt * (0.5 * sph0.alat + dt * acc_lat / 6.0))),
            rad: (sph0.rad + dt * (sph0.vrad + dt * (0.5 * sph0.arad + dt * acc_rad / 6.0))),
            vlon: (sph0.vlon + dt * (sph0.alon + 0.5 * dt * acc_lon)),
            vlat: (sph0.vlat + dt * (sph0.alat + 0.5 * dt * acc_lat)),
            vrad: (sph0.vrad + dt * (sph0.arad + 0.5 * dt * acc_rad)),
            alon: (sph0.alon + dt * acc_lon),
            alat: (sph0.alat + dt * acc_lat),
            arad: (sph0.arad + dt * acc_rad),
        }
    }
}