erfa_rust/
G28_safe.rs

1// G28_safe  Vector/Matrix & Refraction helpers
2//   refco.c → eraRefco_safe
3//   rm2v.c  → eraRm2v_safe
4//   rv2m.c  → eraRv2m_safe
5//   rx.c    → eraRx_safe
6//   rxp.c   → eraRxp_safe
7//   rxpv.c  → eraRxpv_safe
8//   rxr.c   → eraRxr_safe
9//   ry.c    → eraRy_safe
10//   rz.c    → eraRz_safe
11
12pub type ErfaResult<T> = Result<T, ()>;
13
14// Compute refraction coefficients A & B.
15pub fn eraRefco_safe(phpa: f64, tc: f64, rh: f64, wl: f64) -> ErfaResult<(f64, f64)> {
16    // optical/IR if wl ≤ 100 µm
17    let optic = wl <= 100.0;
18
19    // guard inputs, using explicit branching to mirror original macros
20    let mut t = if tc > -150.0 { tc } else { -150.0 };
21    t = if t < 200.0 { t } else { 200.0 };
22    let mut p = if phpa > 0.0 { phpa } else { 0.0 };
23    p = if p < 10_000.0 { p } else { 10_000.0 };
24    let mut r = if rh > 0.0 { rh } else { 0.0 };
25    r = if r < 1.0 { r } else { 1.0 };
26    let mut w = if wl > 0.1 { wl } else { 0.1 };
27    w = if w < 1.0e6 { w } else { 1.0e6 };
28
29    // water-vapour pressure at observer
30    let pw = if p > 0.0 {
31        let ps = 10f64.powf((0.7859 + 0.03477 * t) / (1.0 + 0.00412 * t))
32            * (1.0 + p * (4.5e-6 + 6e-10 * t * t));
33        r * ps / (1.0 - (1.0 - r) * ps / p)
34    } else {
35        0.0
36    };
37
38    // refractivity (n  1) at observer
39    let tk = t + 273.15;
40    let gamma = if optic {
41        let wlsq = w * w;
42        (((77.534_84e-6) + (4.391_08e-7 + 3.666e-9 / wlsq) / wlsq) * p - 11.2684e-6 * pw) / tk
43    } else {
44        (77.6890e-6 * p - (6.3938e-6 - 0.375_463 / tk) * pw) / tk
45    };
46
47    // Stones beta, tweaked
48    let mut beta = 4.4474e-6 * tk;
49    if !optic {
50        beta -= 0.0074 * pw * beta;
51    }
52
53    // Greens refraction constants
54    let refa = gamma * (1.0 - beta);
55    let refb = -gamma * (beta - gamma / 2.0);
56
57    Ok((refa, refb))
58}
59
60// Rotation matrix → rotation vector.
61pub fn eraRm2v_safe(r: &[[f64; 3]; 3]) -> ErfaResult<[f64; 3]> {
62    let x = r[1][2] - r[2][1];
63    let y = r[2][0] - r[0][2];
64    let z = r[0][1] - r[1][0];
65    let s2 = (x * x + y * y + z * z).sqrt();
66
67    let w = if s2 > 0.0 {
68        let c2 = r[0][0] + r[1][1] + r[2][2] - 1.0;
69        let phi = s2.atan2(c2);
70        let f = phi / s2;
71        [x * f, y * f, z * f]
72    } else {
73        [0.0, 0.0, 0.0]
74    };
75
76    Ok(w)
77}
78
79// Rotation vector → rotation matrix.
80pub fn eraRv2m_safe(w: &[f64; 3]) -> ErfaResult<[[f64; 3]; 3]> {
81    let x0 = w[0];
82    let y0 = w[1];
83    let z0 = w[2];
84    let phi = (x0 * x0 + y0 * y0 + z0 * z0).sqrt();
85    let s = phi.sin();
86    let c = phi.cos();
87    let f = 1.0 - c;
88
89    let (mut x, mut y, mut z) = (x0, y0, z0);
90    if phi > 0.0 {
91        x /= phi;
92        y /= phi;
93        z /= phi;
94    }
95
96    let r = [
97        [x * x * f + c, x * y * f + z * s, x * z * f - y * s],
98        [y * x * f - z * s, y * y * f + c, y * z * f + x * s],
99        [z * x * f + y * s, z * y * f - x * s, z * z * f + c],
100    ];
101    Ok(r)
102}
103
104// Rotate matrix about X-axis.
105pub fn eraRx_safe(phi: f64, r: &mut [[f64; 3]; 3]) -> ErfaResult<()> {
106    let s = phi.sin();
107    let c = phi.cos();
108
109    let (r10, r11, r12) = (r[1][0], r[1][1], r[1][2]);
110    let (r20, r21, r22) = (r[2][0], r[2][1], r[2][2]);
111
112    let a10 = c * r10 + s * r20;
113    let a11 = c * r11 + s * r21;
114    let a12 = c * r12 + s * r22;
115    let a20 = -s * r10 + c * r20;
116    let a21 = -s * r11 + c * r21;
117    let a22 = -s * r12 + c * r22;
118
119    r[1][0] = a10;
120    r[1][1] = a11;
121    r[1][2] = a12;
122    r[2][0] = a20;
123    r[2][1] = a21;
124    r[2][2] = a22;
125    Ok(())
126}
127
128// Rotate matrix about Y-axis.
129pub fn eraRy_safe(theta: f64, r: &mut [[f64; 3]; 3]) -> ErfaResult<()> {
130    let s = theta.sin();
131    let c = theta.cos();
132
133    let (r00, r01, r02) = (r[0][0], r[0][1], r[0][2]);
134    let (r20, r21, r22) = (r[2][0], r[2][1], r[2][2]);
135
136    let a00 = c * r00 - s * r20;
137    let a01 = c * r01 - s * r21;
138    let a02 = c * r02 - s * r22;
139    let a20 = s * r00 + c * r20;
140    let a21 = s * r01 + c * r21;
141    let a22 = s * r02 + c * r22;
142
143    r[0][0] = a00;
144    r[0][1] = a01;
145    r[0][2] = a02;
146    r[2][0] = a20;
147    r[2][1] = a21;
148    r[2][2] = a22;
149    Ok(())
150}
151
152// Rotate matrix about Z-axis.
153pub fn eraRz_safe(psi: f64, r: &mut [[f64; 3]; 3]) -> ErfaResult<()> {
154    let s = psi.sin();
155    let c = psi.cos();
156
157    let (r00, r01, r02) = (r[0][0], r[0][1], r[0][2]);
158    let (r10, r11, r12) = (r[1][0], r[1][1], r[1][2]);
159
160    let a00 = c * r00 + s * r10;
161    let a01 = c * r01 + s * r11;
162    let a02 = c * r02 + s * r12;
163    let a10 = -s * r00 + c * r10;
164    let a11 = -s * r01 + c * r11;
165    let a12 = -s * r02 + c * r12;
166
167    r[0][0] = a00;
168    r[0][1] = a01;
169    r[0][2] = a02;
170    r[1][0] = a10;
171    r[1][1] = a11;
172    r[1][2] = a12;
173    Ok(())
174}
175
176// r-matrix × p-vector.
177pub fn eraRxp_safe(r: &[[f64; 3]; 3], p: &[f64; 3]) -> ErfaResult<[f64; 3]> {
178    let mut rp = [0.0_f64; 3];
179    for j in 0..3 {
180        let mut w = 0.0;
181        for i in 0..3 {
182            w += r[j][i] * p[i];
183        }
184        rp[j] = w;
185    }
186    Ok(rp)
187}
188
189// r-matrix × pv-vector.
190pub fn eraRxpv_safe(r: &[[f64; 3]; 3], pv: &[[f64; 3]; 2]) -> ErfaResult<[[f64; 3]; 2]> {
191    let p = eraRxp_safe(r, &pv[0])?;
192    let v = eraRxp_safe(r, &pv[1])?;
193    Ok([p, v])
194}
195
196// Multiply two r-matrices.
197pub fn eraRxr_safe(a: &[[f64; 3]; 3], b: &[[f64; 3]; 3]) -> ErfaResult<[[f64; 3]; 3]> {
198    let mut atb = [[0.0_f64; 3]; 3];
199    for i in 0..3 {
200        for j in 0..3 {
201            let mut w = 0.0;
202            for k in 0..3 {
203                w += a[i][k] * b[k][j];
204            }
205            atb[i][j] = w;
206        }
207    }
208    Ok(atb)
209}
erfa_rust/G28_safe.rs

erfa_rust/
G28_safe.rs
