use rayon::prelude::*;
use crate::error::{Error, Result};
use crate::transform::Transform;
use crate::Point;
use nalgebra::{Matrix3, Vector3};
const CHUNK: usize = 1 << 16;
pub fn sin_cos_deg(deg: f64) -> (f64, f64) {
let r = deg.rem_euclid(360.0);
if r == 0.0 {
(0.0, 1.0)
} else if r == 90.0 {
(1.0, 0.0)
} else if r == 180.0 {
(0.0, -1.0)
} else if r == 270.0 {
(-1.0, 0.0)
} else {
deg.to_radians().sin_cos()
}
}
pub fn rotation(axis: [f64; 3], deg: f64, about: Option<Point>) -> Result<Transform> {
if !deg.is_finite() {
return Err(Error::invalid(format!("rotation angle must be finite, got {deg}")));
}
if axis.iter().any(|v| !v.is_finite()) {
return Err(Error::invalid("rotation axis must be finite"));
}
let n = (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]).sqrt();
if n == 0.0 {
return Err(Error::invalid("rotation axis must not be the zero vector"));
}
let (s, c) = sin_cos_deg(deg);
let nonzero = axis.iter().filter(|v| **v != 0.0).count();
let k = if nonzero == 1 { axis.map(|v| if v == 0.0 { 0.0 } else { v.signum() }) } else { axis.map(|v| v / n) };
let (x, y, z) = (k[0], k[1], k[2]);
let t = 1.0 - c;
let d = |k: f64| if k * k == 1.0 { 1.0 } else { c + k * k * t };
let r = Matrix3::new(
d(x), x * y * t - z * s, x * z * t + y * s,
y * x * t + z * s, d(y), y * z * t - x * s,
z * x * t - y * s, z * y * t + x * s, d(z),
);
let tr = match about {
None => Vector3::zeros(),
Some(a) => {
if a.iter().any(|v| !v.is_finite()) {
return Err(Error::invalid("rotation centre must be finite"));
}
let a = Vector3::new(a[0], a[1], a[2]);
a - r * a
}
};
Ok(Transform::from_rt(r, tr))
}
pub fn axis_from_name(name: &str) -> Result<[f64; 3]> {
match name.trim().to_ascii_lowercase().as_str() {
"x" => Ok([1.0, 0.0, 0.0]),
"y" => Ok([0.0, 1.0, 0.0]),
"z" => Ok([0.0, 0.0, 1.0]),
other => Err(Error::invalid(format!("axis must be \"x\", \"y\", \"z\" or a 3-vector, got {other:?}"))),
}
}
pub fn is_pure_translation(t: &Transform) -> bool {
let m = &t.0;
(0..3).all(|r| (0..3).all(|c| m[(r, c)] == if r == c { 1.0 } else { 0.0 }))
&& m[(3, 0)] == 0.0
&& m[(3, 1)] == 0.0
&& m[(3, 2)] == 0.0
&& m[(3, 3)] == 1.0
}
pub fn apply_in_place(xyz: &mut [Point], t: &Transform) {
let m = t.0;
if is_pure_translation(t) {
let (dx, dy, dz) = (m[(0, 3)], m[(1, 3)], m[(2, 3)]);
xyz.par_chunks_mut(CHUNK).for_each(|chunk| {
for p in chunk {
p[0] += dx;
p[1] += dy;
p[2] += dz;
}
});
return;
}
xyz.par_chunks_mut(CHUNK).for_each(|chunk| {
for p in chunk {
*p = t.apply(p);
}
});
}
pub fn apply(xyz: &[Point], t: &Transform) -> Vec<Point> {
let mut out = xyz.to_vec();
apply_in_place(&mut out, t);
out
}
pub fn recentre_origin(xyz: &[Point]) -> Point {
let lo = xyz
.par_chunks(CHUNK)
.map(|chunk| {
let mut lo = [f64::INFINITY; 3];
for p in chunk {
for k in 0..3 {
if p[k].is_finite() && p[k] < lo[k] {
lo[k] = p[k];
}
}
}
lo
})
.reduce(|| [f64::INFINITY; 3], |a, b| [a[0].min(b[0]), a[1].min(b[1]), a[2].min(b[2])]);
lo.map(|v| if v.is_finite() { v.floor() } else { 0.0 })
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn quarter_turns_are_exact() {
let t = rotation([0.0, 0.0, 1.0], 90.0, None).unwrap();
assert_eq!(t.apply(&[1.0, 2.0, 3.0]), [-2.0, 1.0, 3.0]);
let t = rotation([1.0, 0.0, 0.0], -90.0, None).unwrap();
assert_eq!(t.apply(&[1.0, 2.0, 3.0]), [1.0, 3.0, -2.0]);
let t = rotation([0.0, 2.0, 0.0], 180.0, Some([1.0, 0.0, 1.0])).unwrap();
assert_eq!(t.apply(&[2.0, 5.0, 1.0]), [0.0, 5.0, 1.0]);
for k in -8..=8 {
let (s, c) = sin_cos_deg(90.0 * k as f64);
assert!(s.abs() == 0.0 || s.abs() == 1.0);
assert!(c.abs() == 0.0 || c.abs() == 1.0);
}
}
#[test]
fn matches_rotation_z_and_rpy() {
let a = rotation([0.0, 0.0, 1.0], 33.0, None).unwrap();
let b = Transform::rotation_z(33.0);
assert!((a.0 - b.0).abs().max() < 1e-15);
let a = rotation([1.0, 0.0, 0.0], 12.0, None).unwrap();
let b = Transform::from_roll_pitch_yaw(12.0, 0.0, 0.0);
assert!((a.0 - b.0).abs().max() < 1e-15);
let a = rotation([0.0, 1.0, 0.0], -7.0, None).unwrap();
let b = Transform::from_roll_pitch_yaw(0.0, -7.0, 0.0);
assert!((a.0 - b.0).abs().max() < 1e-15);
}
#[test]
fn arbitrary_axis_is_a_rotation_about_that_axis() {
let axis = [1.0, 2.0, -0.5];
let t = rotation(axis, 41.0, Some([10.0, -3.0, 2.0])).unwrap();
let r = t.rotation();
assert!((r.transpose() * r - Matrix3::identity()).abs().max() < 1e-15);
assert!((r.determinant() - 1.0).abs() < 1e-15);
let p = [10.0 + 2.0, -3.0 + 4.0, 2.0 - 1.0];
let q = t.apply(&p);
for k in 0..3 {
assert!((p[k] - q[k]).abs() < 1e-12);
}
}
#[test]
fn round_trips_do_not_drift() {
let pts: Vec<Point> = (0..1000).map(|i| [500_000.0 + i as f64 * 0.37, 6_900_000.0 - i as f64 * 0.11, 100.0 + (i % 7) as f64]).collect();
let mut xyz = pts.clone();
let about = Some([500_100.0, 6_899_950.0, 0.0]);
for _ in 0..100 {
apply_in_place(&mut xyz, &rotation([0.0, 0.0, 1.0], 17.3, about).unwrap());
apply_in_place(&mut xyz, &rotation([0.0, 0.0, 1.0], -17.3, about).unwrap());
apply_in_place(&mut xyz, &Transform::translation(-500_000.0, -6_900_000.0, -100.0));
apply_in_place(&mut xyz, &Transform::translation(500_000.0, 6_900_000.0, 100.0));
}
let worst = xyz.iter().zip(&pts).map(|(a, b)| (0..3).map(|k| (a[k] - b[k]).abs()).fold(0.0, f64::max)).fold(0.0, f64::max);
assert!(worst < 1e-6, "drift {worst}");
let mut xyz = pts.clone();
apply_in_place(&mut xyz, &Transform::translation(-500_000.0, -6_900_000.0, -100.0));
apply_in_place(&mut xyz, &Transform::translation(500_000.0, 6_900_000.0, 100.0));
assert_eq!(xyz, pts);
}
#[test]
fn apply_matches_transform_apply() {
let t = Transform::from_roll_pitch_yaw(3.0, -2.0, 40.0).compose(&Transform::translation(1.0, 2.0, 3.0));
let pts: Vec<Point> = (0..200_000).map(|i| [i as f64 * 0.001, (i % 97) as f64, -(i as f64).sqrt()]).collect();
let out = apply(&pts, &t);
for (p, q) in pts.iter().zip(&out) {
assert_eq!(t.apply(p), *q);
}
}
#[test]
fn origin_ignores_nan_and_handles_empty() {
assert_eq!(recentre_origin(&[]), [0.0; 3]);
let pts = vec![[f64::NAN, 5.5, -0.5], [500_000.7, 6.2, 3.0], [500_001.0, f64::INFINITY, 2.0]];
assert_eq!(recentre_origin(&pts), [500_000.0, 5.0, -1.0]);
assert_eq!(recentre_origin(&[[f64::NAN; 3]]), [0.0; 3]);
}
#[test]
fn invalid_rotations() {
assert!(rotation([0.0; 3], 10.0, None).is_err());
assert!(rotation([0.0, 0.0, 1.0], f64::NAN, None).is_err());
assert!(rotation([0.0, 0.0, 1.0], 1.0, Some([f64::NAN, 0.0, 0.0])).is_err());
assert!(axis_from_name("w").is_err());
assert_eq!(axis_from_name(" Y ").unwrap(), [0.0, 1.0, 0.0]);
}
}