use nalgebra::{Matrix3, Matrix4, Vector3};
use crate::error::{Error, Result};
use crate::Point;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Transform(pub Matrix4<f64>);
impl Default for Transform {
fn default() -> Self {
Transform(Matrix4::identity())
}
}
impl Transform {
pub fn identity() -> Self {
Self::default()
}
pub fn from_rt(r: Matrix3<f64>, t: Vector3<f64>) -> Self {
let mut m = Matrix4::identity();
m.fixed_view_mut::<3, 3>(0, 0).copy_from(&r);
m.fixed_view_mut::<3, 1>(0, 3).copy_from(&t);
Transform(m)
}
pub fn from_row_major(v: &[f64]) -> Result<Self> {
if v.len() != 16 {
return Err(Error::invalid("transform needs 16 values"));
}
Ok(Transform(Matrix4::from_row_slice(v)))
}
pub fn to_row_major(&self) -> [f64; 16] {
let mut out = [0.0; 16];
for r in 0..4 {
for c in 0..4 {
out[r * 4 + c] = self.0[(r, c)];
}
}
out
}
pub fn translation(dx: f64, dy: f64, dz: f64) -> Self {
Self::from_rt(Matrix3::identity(), Vector3::new(dx, dy, dz))
}
pub fn rotation_z(deg: f64) -> Self {
let a = deg.to_radians();
let (s, c) = a.sin_cos();
Self::from_rt(Matrix3::new(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0), Vector3::zeros())
}
pub fn from_roll_pitch_yaw(roll: f64, pitch: f64, yaw: f64) -> Self {
let rx = Matrix3::new(
1.0, 0.0, 0.0,
0.0, roll.to_radians().cos(), -roll.to_radians().sin(),
0.0, roll.to_radians().sin(), roll.to_radians().cos(),
);
let ry = Matrix3::new(
pitch.to_radians().cos(), 0.0, pitch.to_radians().sin(),
0.0, 1.0, 0.0,
-pitch.to_radians().sin(), 0.0, pitch.to_radians().cos(),
);
let rz = Matrix3::new(
yaw.to_radians().cos(), -yaw.to_radians().sin(), 0.0,
yaw.to_radians().sin(), yaw.to_radians().cos(), 0.0,
0.0, 0.0, 1.0,
);
Self::from_rt(rz * ry * rx, Vector3::zeros())
}
pub fn rotation(&self) -> Matrix3<f64> {
self.0.fixed_view::<3, 3>(0, 0).into_owned()
}
pub fn translation_vec(&self) -> Vector3<f64> {
self.0.fixed_view::<3, 1>(0, 3).into_owned()
}
#[inline]
pub fn apply(&self, p: &Point) -> Point {
let m = &self.0;
[
m[(0, 0)] * p[0] + m[(0, 1)] * p[1] + m[(0, 2)] * p[2] + m[(0, 3)],
m[(1, 0)] * p[0] + m[(1, 1)] * p[1] + m[(1, 2)] * p[2] + m[(1, 3)],
m[(2, 0)] * p[0] + m[(2, 1)] * p[1] + m[(2, 2)] * p[2] + m[(2, 3)],
]
}
#[inline]
pub fn apply_dir(&self, d: &Point) -> Point {
let m = &self.0;
[
m[(0, 0)] * d[0] + m[(0, 1)] * d[1] + m[(0, 2)] * d[2],
m[(1, 0)] * d[0] + m[(1, 1)] * d[1] + m[(1, 2)] * d[2],
m[(2, 0)] * d[0] + m[(2, 1)] * d[1] + m[(2, 2)] * d[2],
]
}
pub fn compose(&self, other: &Transform) -> Transform {
Transform(self.0 * other.0)
}
pub fn inverse(&self) -> Result<Transform> {
self.0.try_inverse().map(Transform).ok_or_else(|| Error::invalid("singular transform"))
}
pub fn read_matrix_file(path: impl AsRef<std::path::Path>) -> Result<Self> {
let text = std::fs::read_to_string(path.as_ref())?;
let vals: Vec<f64> = text
.split_whitespace()
.map(|t| t.parse::<f64>())
.collect::<std::result::Result<_, _>>()
.map_err(|e| Error::file(path.as_ref(), format!("bad matrix value: {e}")))?;
if vals.len() != 16 {
return Err(Error::file(path.as_ref(), format!("expected 16 values, got {}", vals.len())));
}
Self::from_row_major(&vals)
}
}
#[inline]
pub(crate) fn sub(a: &Point, b: &Point) -> Point {
[a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}
#[inline]
pub(crate) fn dot(a: &Point, b: &Point) -> f64 {
a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
#[inline]
pub(crate) fn norm(a: &Point) -> f64 {
dot(a, a).sqrt()
}
#[inline]
pub(crate) fn scale(a: &Point, s: f64) -> Point {
[a[0] * s, a[1] * s, a[2] * s]
}
#[inline]
pub(crate) fn add(a: &Point, b: &Point) -> Point {
[a[0] + b[0], a[1] + b[1], a[2] + b[2]]
}
#[inline]
pub(crate) fn cross(a: &Point, b: &Point) -> Point {
[a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
}
#[inline]
pub(crate) fn normalize(a: &Point) -> Point {
let n = norm(a);
if n > 0.0 {
scale(a, 1.0 / n)
} else {
[0.0, 0.0, 1.0]
}
}