sofars/vm/
pv2s.rs

1/// Convert position/velocity from Cartesian to spherical coordinates.
2pub fn pv2s(pv: &[[f64; 3]; 2]) -> (f64, f64, f64, f64, f64, f64) {
3    let (theta, phi, r, td, pd, rd): (f64, f64, f64, f64, f64, f64);
4
5    let (mut x, mut y, mut z): (f64, f64, f64);
6    let (xd, yd, zd, mut rxy2, rxy): (f64, f64, f64, f64, f64);
7    let (mut r2, rtrue, mut rw, xyp): (f64, f64, f64, f64);
8
9    /* Components of position/velocity vector. */
10    x = pv[0][0];
11    y = pv[0][1];
12    z = pv[0][2];
13    xd = pv[1][0];
14    yd = pv[1][1];
15    zd = pv[1][2];
16
17    /* Component of r in XY plane squared. */
18    rxy2 = x * x + y * y;
19
20    /* Modulus squared. */
21    r2 = rxy2 + z * z;
22
23    /* Modulus. */
24    rtrue = r2.sqrt();
25
26    /* If null vector, move the origin along the direction of movement. */
27    rw = rtrue;
28    if rtrue == 0.0 {
29        x = xd;
30        y = yd;
31        z = zd;
32        rxy2 = x * x + y * y;
33        r2 = rxy2 + z * z;
34        rw = r2.sqrt();
35    }
36
37    /* Position and velocity in spherical coordinates. */
38    rxy = rxy2.sqrt();
39    xyp = x * xd + y * yd;
40    if rxy2 != 0.0 {
41        theta = y.atan2(x);
42        phi = z.atan2(rxy);
43        td = (x * yd - y * xd) / rxy2;
44        pd = (zd * rxy2 - z * xyp) / (r2 * rxy);
45    } else {
46        theta = 0.0;
47        phi = if z != 0.0 { z.atan2(rxy) } else { 0.0 };
48        td = 0.0;
49        pd = 0.0;
50    }
51    r = rtrue;
52    rd = if rw != 0.0 { (xyp + z * zd) / rw } else { 0.0 };
53
54    (theta, phi, r, td, pd, rd)
55}
sofars/vm/pv2s.rs

sofars/vm/
pv2s.rs
