use std::f64::consts::TAU;
#[derive(Debug)]
pub struct RefToLocalRotMatrix {
r11: f64,
r12: f64,
r13: f64,
r21: f64,
r22: f64,
r23: f64,
r31: f64,
r32: f64,
r33: f64,
}
impl RefToLocalRotMatrix {
pub fn from_center(lon: f64, lat: f64) -> RefToLocalRotMatrix {
let (sa, ca) = lon.sin_cos(); let (sd, cd) = lat.sin_cos(); RefToLocalRotMatrix {
r11: ca * cd,
r12: sa * cd,
r13: sd,
r21: -sa,
r22: ca,
r23: 0.0,
r31: -ca * sd,
r32: -sa * sd,
r33: cd,
}
}
pub fn to_global_xyz(&self, x: f64, y: f64, z: f64) -> (f64, f64, f64) {
(
self.r11 * x + self.r21 * y + self.r31 * z,
self.r12 * x + self.r22 * y + self.r32 * z,
self.r13 * x + self.r23 * y + self.r33 * z,
)
}
pub fn to_global_coo(&self, x: f64, y: f64, z: f64) -> (f64, f64) {
let (x, y, z) = self.to_global_xyz(x, y, z);
xyz_to_lonlat(x, y, z)
}
}
fn xyz_to_lonlat(x: f64, y: f64, z: f64) -> (f64, f64) {
let r2 = x * x + y * y;
let lat = z.atan2(r2.sqrt());
let r2 = y.atan2(x);
let lon = if r2 < 0.0 { TAU + r2 } else { r2 };
(lon, lat)
}