rkepler 0.4.1

/*
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.
*/

//! Position, velocity and acceleration vectors in Cartesian coordinates.

/// Represent position vector (p-vector)
pub mod vec_p {

    /// Position vector in Cartesian coordinates
    #[derive(Debug)]
    pub struct VecP {
        /// position x component
        pub x: f64,
        /// position y component
        pub y: f64,
        /// position z component
        pub z: f64,
    }

    impl VecP {
        /// Modulus of position vector.
        /// # Arguments
        /// # Returns
        /// modulus (length) of vector
        pub fn modul(&self) -> f64 {
            (self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
        }
    }

    /// Position vector addition.
    /// # Arguments
    /// * `a` first vector
    /// * `b` second vector
    /// # Returns
    /// a + b
    pub fn add(a: &VecP, b: &VecP) -> VecP {
        VecP {
            x: a.x + b.x,
            y: a.y + b.y,
            z: a.z + b.z,
        }
    }

    /// Position vector plus scaled vector.
    /// # Arguments
    /// * `a` first vector
    /// * `s` scalar (multiplier for b)
    /// * `b` second vector
    /// # Returns
    /// a + s*b
    pub fn add_scaled(a: &VecP, s: f64, b: &VecP) -> VecP {
        VecP {
            x: a.x + s * b.x,
            y: a.y + s * b.y,
            z: a.z + s * b.z,
        }
    }

    /// Position vector subtraction.
    /// # Arguments
    /// * `a` first vector
    /// * `b` second vector
    /// # Returns
    /// a - b
    pub fn sub(a: &VecP, b: &VecP) -> VecP {
        VecP {
            x: a.x - b.x,
            y: a.y - b.y,
            z: a.z - b.z,
        }
    }

    /// Position vector inner (=scalar=dot) product.
    /// # Arguments
    /// * `a` first vector
    /// * `b` second vector
    /// # Returns
    /// a . b
    pub fn dot(a: &VecP, b: &VecP) -> f64 {
        a.x * b.x + a.y * b.y + a.z * b.z
    }

    /// Position vector outer (=vector=cross) product.
    /// # Arguments
    /// * `a` first vector
    /// * `b` second vector
    /// # Returns
    /// a x b
    pub fn cross(a: &VecP, b: &VecP) -> VecP {
        VecP {
            x: a.y * b.z - a.z * b.y,
            y: a.z * b.x - a.x * b.z,
            z: a.x * b.y - a.y * b.x,
        }
    }

    /// Multiply a position vector by a scalar.
    /// # Arguments
    /// * `s` scalar
    /// * `a` position vector
    /// # Returns
    /// s * p
    pub fn scale(s: f64, a: &VecP) -> VecP {
        VecP {
            x: s * a.x,
            y: s * a.y,
            z: s * a.z,
        }
    }

    /// Angular separation between two position vectors.
    /// # Arguments
    /// * `a` first position vector (not necessarily unit length)
    /// * `b` second position vector (not necessarily unit length)
    /// # Returns
    /// angular separation in radians, always positive
    pub fn sep_angl(a: &VecP, b: &VecP) -> f64 {
        let ss = cross(a, b).modul();
        let cs = dot(a, b);
        if (ss != 0.0) || (cs != 0.0) {
            ss.atan2(cs)
        } else {
            0.0
        }
    }

    /// Position-angle from two position vectors.
    /// # Arguments
    /// * `a` direction of reference point
    /// * `b` direction of point whose PA is required
    /// # Returns
    /// position angle of b with respect to a in radians, range -pi to +pi
    pub fn pos_angl(a: &VecP, b: &VecP) -> f64 {
        //TODO
        let am = a.modul();
        let au = scale(1.0 / am, a);
        let bm = b.modul();
        if (am == 0.0) || (bm == 0.0) {
            0.0
        } else {
            let eta: VecP = VecP {
                x: -a.x * a.z,
                y: -a.y * a.z,
                z: a.x * a.x + a.y * a.y,
            };
            let xi = cross(&eta, &au);
            let ab = sub(&b, &a);
            let st = dot(&ab, &xi);
            let ct = dot(&ab, &eta);
            if (st == 0.0) && (ct == 0.0) {
                0.0
            } else {
                st.atan2(ct)
            }
        }
    }

    pub fn vel(t0: f64, p0: &VecP, t1: f64, p1: &VecP) -> VecP {
        let dt = t1 - t0;
        VecP {
            x: (p1.x - p0.x) / dt,
            y: (p1.y - p0.y) / dt,
            z: (p1.z - p0.z) / dt,
        }
    }

    pub fn interp(t: f64, t0: f64, p0: &VecP, t1: f64, p1: &VecP) -> VecP {
        let dt = t - t0;
        let vel = vel(t0, p0, t1, p1);
        add_scaled(p0, dt, &vel)
    }
}

/// Represent position and velocity vectors in Cartesian coordinates (pv-vector)
pub mod vec_pv {
    use super::vec_p::VecP;
    use crate::math::scalar::scl_sv::SclSv;

    /// Position and velocity vectors
    #[derive(Debug)]
    pub struct VecPv {
        /// position x component
        pub x: f64,
        /// position y component
        pub y: f64,
        /// position z component
        pub z: f64,
        /// velocity x component
        pub vx: f64,
        /// velocity y component
        pub vy: f64,
        /// velocity z component
        pub vz: f64,
    }

    impl VecPv {
        /// Modulus and his velocity of position component of pv-vector.
        /// # Arguments
        /// # Returns
        /// s: Modulus of position component, v: velocity of this modulus
        pub fn modul(&self) -> SclSv {
            let m = (self.x * self.x + self.y * self.y + self.z * self.z).sqrt();
            SclSv {
                s: m,
                v: (self.x * self.vx + self.y * self.vy + self.z * self.vz) / m,
            }
        }

        /// Modulus of velocity component of pv-vector.
        /// # Arguments
        /// # Returns
        /// modulus of velocity component
        pub fn modul_v(&self) -> f64 {
            let v = VecP {
                x: self.vx,
                y: self.vy,
                z: self.vz,
            };
            v.modul()
        }
    }

    /// Add one pv-vector to another.
    /// # Arguments
    /// * `a` first pv-vector
    /// * `b` second pv-vector
    /// # Returns
    /// a + b
    pub fn add(a: &VecPv, b: &VecPv) -> VecPv {
        VecPv {
            x: a.x + b.x,
            y: a.y + b.y,
            z: a.z + b.z,
            vx: a.vx + b.vx,
            vy: a.vy + b.vy,
            vz: a.vz + b.vz,
        }
    }

    /// Subtract one pv-vector from another.
    /// # Arguments
    /// * `a` first pv-vector
    /// * `b`: second pv-vector
    /// # Returns
    /// a - b
    pub fn sub(a: &VecPv, b: &VecPv) -> VecPv {
        VecPv {
            x: a.x - b.x,
            y: a.y - b.y,
            z: a.z - b.z,
            vx: a.vx - b.vx,
            vy: a.vy - b.vy,
            vz: a.vz - b.vz,
        }
    }

    /// Inner (=scalar=dot) product of two pv-vectors.
    /// # Arguments
    /// * `a` first pv-vector
    /// * `b` second pv-vector
    /// # Returns
    /// s: ap . bp , v: ap . bv + av . bp
    pub fn dot(a: &VecPv, b: &VecPv) -> SclSv {
        SclSv {
            s: a.x * b.x + a.y * b.y + a.z * b.z,
            v: a.x * b.vx + a.y * b.vy + a.z * b.vz + a.vx * b.x + a.vy * b.y + a.vz * b.z,
        }
    }

    /// Outer (=vector=cross) product of two pv-vectors.
    /// # Arguments
    /// * `a` first pv-vector
    /// * `b` second pv-vector
    /// # Returns
    /// position: ap x bp, velocity: ap x bv + av x bp
    pub fn cross(a: &VecPv, b: &VecPv) -> VecPv {
        VecPv {
            x: a.y * b.z - a.z * b.y,
            y: a.z * b.x - a.x * b.z,
            z: a.x * b.y - a.y * b.x,
            vx: a.y * b.vz + a.vy * b.z - a.z * b.vy - a.vz * b.y,
            vy: a.z * b.vx + a.vz * b.x - a.x * b.vz - a.vx * b.z,
            vz: a.x * b.vy + a.vx * b.y - a.y * b.vx - a.vy * b.x,
        }
    }

    /// Multiply a pv-vector by a scalar.
    /// # Arguments
    /// * `s` scalar
    /// * `a` pv-vector
    /// # Returns
    /// s * pv
    pub fn scale(s: f64, a: &VecPv) -> VecPv {
        VecPv {
            x: s * a.x,
            y: s * a.y,
            z: s * a.z,
            vx: s * a.vx,
            vy: s * a.vy,
            vz: s * a.vz,
        }
    }

    /// Multiply a pv-vector by two scalars.
    /// # Arguments
    /// * `sp` scalar to multiply position component
    /// * `sv` scalar to multiply velocity component
    /// * `a` pv-vector
    /// # Returns
    /// p scaled by sp, v scaled by sv
    pub fn scale_other(sp: f64, sv: f64, a: &VecPv) -> VecPv {
        VecPv {
            x: sp * a.x,
            y: sp * a.y,
            z: sp * a.z,
            vx: sv * a.vx,
            vy: sv * a.vy,
            vz: sv * a.vz,
        }
    }

    /// Angular separation between two pv-vector.
    /// # Arguments
    /// * `a` first pv-vector (not necessarily unit length)
    /// * `b` second pv-vector (not necessarily unit length)
    /// # Returns
    /// s: angular separation in radians, always positive
    /// v: his velocity
    pub fn sep_angl(a: &VecPv, b: &VecPv) -> SclSv {
        let ss = cross(a, b).modul();
        let cs = dot(a, b);
        if (ss.s != 0.0) || (cs.s != 0.0) {
            SclSv {
                s: ss.s.atan2(cs.s),
                v: (ss.v * cs.s - ss.s * cs.v) / (ss.s * ss.s + cs.s * cs.s),
            }
        } else {
            SclSv { s: 0.0, v: 0.0 }
        }
    }

    pub fn update(t: f64, t0: f64, pva0: &VecPv) -> VecPv {
        let dt = t - t0;
        VecPv {
            x: (pva0.x + dt * pva0.vx),
            y: (pva0.y + dt * pva0.vy),
            z: (pva0.z + dt * pva0.vz),
            vx: (pva0.vx),
            vy: (pva0.vy),
            vz: (pva0.vz),
        }
    }

    /// Interpolate pv-vector in time `t`
    /// # Arguments
    /// * `t` moment for interpolation (t0 < t < t1)
    /// * `t0` moment 0 (same dimension as time in velocity)
    /// * `pv0` pv-vector in moment `t0`
    /// * `t1` moment 1 (same dimension as time in velocity)
    /// * `pv1` pv-vector in moment `t1`
    /// # Returns
    /// pv-vector in moment `t`
    pub fn interp(t: f64, t0: f64, pv0: &VecPv, t1: f64, pv1: &VecPv) -> VecPv {
        let dt = t - t0;
        let dta = t1 - t0;
        let dacc = super::vec_p::VecP {
            x: (pv1.vx - pv0.vx) / dta,
            y: (pv1.vy - pv0.vy) / dta,
            z: (pv1.vz - pv0.vz) / dta,
        };
        VecPv {
            x: (pv0.x + dt * (pv0.vx + 0.5 * dt * dacc.x)),
            y: (pv0.y + dt * (pv0.vy + 0.5 * dt * dacc.y)),
            z: (pv0.z + dt * (pv0.vz + 0.5 * dt * dacc.z)),
            vx: (pv0.vx + dt * dacc.x),
            vy: (pv0.vy + dt * dacc.y),
            vz: (pv0.vz + dt * dacc.z),
        }
    }
}

/// Represent position, velocity and acceleration vectors (pva-vector)
pub mod vec_pva {

    use crate::math::scalar::scl_sv::SclSv;
    use crate::math::scalar::scl_sva::SclSva;

    /// Position,velocity and acceleration vectors in Cartesian coordinates
    #[derive(Debug)]
    pub struct VecPva {
        /// position x component
        pub x: f64,
        /// position y component
        pub y: f64,
        /// position z component
        pub z: f64,
        /// velocity x component
        pub vx: f64,
        /// velocity y component
        pub vy: f64,
        /// velocity z component
        pub vz: f64,
        /// acceleration x component
        pub ax: f64,
        /// acceleration y component
        pub ay: f64,
        /// acceleration z component
        pub az: f64,
    }

    impl VecPva {
        /// Modulus and his velocity and acceleration of position component of pva-vector.
        /// # Arguments
        /// # Returns
        /// s: Modulus of position component, v: velocity of this modulus, a: acceleration of this modulus
        pub fn modul(&self) -> SclSva {
            let m = (self.x * self.x + self.y * self.y + self.z * self.z).sqrt();
            let mv2 = self.x * self.vx + self.y * self.vy + self.z * self.vz;
            SclSva {
                s: (m),
                v: (mv2 / m),
                a: ((self.x * self.ax
                    + self.vx * self.vx
                    + self.y * self.ay
                    + self.vy * self.vy
                    + self.z * self.az
                    + self.vz * self.vz)
                    / m
                    - (mv2 * mv2) / (m * m * m)),
            }
        }

        pub fn modul_v(&self) -> SclSv {
            let a = super::vec_pv::VecPv {
                x: self.vx,
                y: self.vy,
                z: self.vz,
                vx: self.ax,
                vy: self.ay,
                vz: self.az,
            };
            a.modul()
        }

        /// Modulus of acceleration component of pva-vector.
        /// # Arguments
        /// # Returns
        /// modulus of acceleration component
        pub fn modul_a(&self) -> f64 {
            let a = super::vec_p::VecP {
                x: self.ax,
                y: self.ay,
                z: self.az,
            };
            a.modul()
        }
    }

    /// Add one pva-vector from another.
    /// # Arguments
    /// * `a` first pva-vector
    /// * `b`: second pva-vector
    /// # Returns
    /// a - b
    pub fn add(a: &VecPva, b: &VecPva) -> VecPva {
        VecPva {
            x: (a.x + b.x),
            y: (a.y + b.y),
            z: (a.z + b.z),
            vx: (a.vx + b.vx),
            vy: (a.vy + b.vy),
            vz: (a.vz + b.vz),
            ax: (a.ax + b.ax),
            ay: (a.ay + b.ay),
            az: (a.az + b.az),
        }
    }

    /// Subtract one pva-vector from another.
    /// # Arguments
    /// * `a` first pva-vector
    /// * `b`: second pva-vector
    /// # Returns
    /// a - b
    pub fn sub(a: &VecPva, b: &VecPva) -> VecPva {
        VecPva {
            x: (a.x - b.x),
            y: (a.y - b.y),
            z: (a.z - b.z),
            vx: (a.vx - b.vx),
            vy: (a.vy - b.vy),
            vz: (a.vz - b.vz),
            ax: (a.ax - b.ax),
            ay: (a.ay - b.ay),
            az: (a.az - b.az),
        }
    }

    /// Inner (=scalar=dot) product of two pva-vectors.
    /// # Arguments
    /// * `a` first pva-vector
    /// * `b` second pva-vector
    /// # Returns
    /// s: ap . bp ,
    /// v: ap . bv + av . bp,
    /// a: av . bv + ap . ba + aa . bp + av . bv
    pub fn dot(a: &VecPva, b: &VecPva) -> SclSva {
        SclSva {
            s: (a.x * b.x + a.y * b.y + a.z * b.z),
            v: (a.x * b.vx + a.y * b.vy + a.z * b.vz + a.vx * b.x + a.vy * b.y + a.vz * b.z),
            a: (a.x * b.ax
                + 2.0 * a.vx * b.vx
                + a.y * b.ay
                + 2.0 * a.vy * b.vy
                + a.z * b.az
                + 2.0 * a.vz * b.vz
                + a.ax * b.x
                + a.ay * b.y
                + a.az * b.z),
        }
    }

    /// Outer (=vector=cross) product of two pva-vectors.
    /// # Arguments
    /// * `a` first pva-vector
    /// * `b` second pva-vector
    /// # Returns
    /// p: ap x bp,
    /// v: ap x bv + av x bp,
    /// a: av x bv + ap * ba + aa x bp + av x bv
    pub fn cross(a: &VecPva, b: &VecPva) -> VecPva {
        VecPva {
            x: (a.y * b.z - a.z * b.y),
            y: (a.z * b.x - a.x * b.z),
            z: (a.x * b.y - a.y * b.x),
            vx: (a.y * b.vz + a.vy * b.z - a.z * b.vy - a.vz * b.y),
            vy: (a.z * b.vx + a.vz * b.x - a.x * b.vz - a.vx * b.z),
            vz: (a.x * b.vy + a.vx * b.y - a.y * b.vx - a.vy * b.x),
            ax: (2.0 * a.vy * b.vz + a.y * b.az + a.ay * b.z
                - 2.0 * a.vz * b.vy
                - a.z * b.ay
                - a.az * b.y),
            ay: (2.0 * a.vz * b.vx + a.z * b.ax + a.az * b.x
                - 2.0 * a.vx * b.vz
                - a.x * b.az
                - a.ax * b.z),
            az: (2.0 * a.vx * b.vy + a.x * b.ay + a.ax * b.y
                - 2.0 * a.vy * b.vx
                - a.y * b.ax
                - a.ay * b.x),
        }
    }

    /// Multiply a pva-vector by a scalar.
    /// # Arguments
    /// * `s` scalar
    /// * `a` pva-vector
    /// # Returns
    /// s * pva
    pub fn scale(s: f64, a: &VecPva) -> VecPva {
        VecPva {
            x: (s * a.x),
            y: (s * a.y),
            z: (s * a.z),
            vx: (s * a.vx),
            vy: (s * a.vy),
            vz: (s * a.vz),
            ax: (s * a.ax),
            ay: (s * a.ay),
            az: (s * a.az),
        }
    }

    /// Multiply a pv-vector by two scalars.
    /// # Arguments
    /// * `sp` scalar to multiply position component
    /// * `sv` scalar to multiply velocity component
    /// * `a` pv-vector
    /// # Returns
    /// p scaled by sp, v scaled by sv
    pub fn scale_other(sp: f64, sv: f64, sa: f64, a: &VecPva) -> VecPva {
        VecPva {
            x: (sp * a.x),
            y: (sp * a.y),
            z: (sp * a.z),
            vx: (sv * a.vx),
            vy: (sv * a.vy),
            vz: (sv * a.vz),
            ax: (sa * a.ax),
            ay: (sa * a.ay),
            az: (sa * a.az),
        }
    }

    /// Angular separation between two pva-vector.
    /// # Arguments
    /// * `a` first pva-vector (not necessarily unit length)
    /// * `b` second pva-vector (not necessarily unit length)
    /// # Returns
    /// s: angular separation in radians, always positive
    /// v: his velocity
    /// a: his acceleration
    pub fn sep_angl(a: &VecPva, b: &VecPva) -> SclSva {
        let ss = cross(a, b).modul();
        let cs = dot(a, b);
        if (ss.s != 0.0) || (cs.s != 0.0) {
            let svs = ss.v * cs.s - ss.s * cs.v;
            let norm = ss.s * ss.s + cs.s * cs.s;
            SclSva {
                s: (ss.s.atan2(cs.s)),
                v: (svs / norm),
                a: ((norm * (cs.s * ss.a - ss.s * cs.a) - 2.0 * svs * (cs.s * cs.v + ss.s * ss.v))
                    / (norm * norm)),
            }
        } else {
            SclSva {
                s: 0.0,
                v: 0.0,
                a: 0.0,
            }
        }
    }

    pub fn update(t: f64, t0: f64, pva0: &VecPva) -> VecPva {
        let dt = t - t0;
        VecPva {
            x: (pva0.x + dt * (pva0.vx + dt * 0.5 * pva0.ax)),
            y: (pva0.y + dt * (pva0.vy + dt * 0.5 * pva0.ay)),
            z: (pva0.z + dt * (pva0.vz + dt * 0.5 * pva0.az)),
            vx: (pva0.vx + dt * pva0.ax),
            vy: (pva0.vy + dt * pva0.ay),
            vz: (pva0.vz + dt * pva0.az),
            ax: (pva0.ax),
            ay: (pva0.ay),
            az: (pva0.az),
        }
    }

    /// Interpolate pva-vector in time `t`
    /// # Arguments
    /// * `t` moment for interpolation (t0 < t < t1)
    /// * `t0` moment 0 (same dimension as time in velocity)
    /// * `pva0` pva-vector in moment `t0`
    /// * `t1` moment 1 (same dimension as time in velocity)
    /// * `pva1` pva-vector in moment `t1`
    /// # Returns
    /// pv-vector in moment `mjd`
    pub fn interp(t: f64, t0: f64, pva0: &VecPva, t1: f64, pva1: &VecPva) -> VecPva {
        let dt = t - t0;
        let dta = t1 - t0;
        let dacc = super::vec_p::VecP {
            x: (pva1.ax - pva0.ax) / dta,
            y: (pva1.ay - pva0.ay) / dta,
            z: (pva1.az - pva0.az) / dta,
        };
        VecPva {
            x: (pva0.x + dt * (pva0.vx + dt * (0.5 * pva0.ax + dt * dacc.x / 6.0))),
            y: (pva0.y + dt * (pva0.vy + dt * (0.5 * pva0.ay + dt * dacc.y / 6.0))),
            z: (pva0.z + dt * (pva0.vz + dt * (0.5 * pva0.az + dt * dacc.z / 6.0))),
            vx: (pva0.vx + dt * (pva0.ax + 0.5 * dt * dacc.x)),
            vy: (pva0.vy + dt * (pva0.ay + 0.5 * dt * dacc.y)),
            vz: (pva0.vz + dt * (pva0.az + 0.5 * dt * dacc.z)),
            ax: (pva0.ax + dt * dacc.x),
            ay: (pva0.ay + dt * dacc.y),
            az: (pva0.az + dt * dacc.z),
        }
    }
}