Skip to main content

sylva_rs/
transform.rs

1// Sylva: LiDAR processing for forest ecology and remote sensing research.
2// Copyright (C) 2026 Tim Devereux, The University of Queensland.
3// Free software under the GNU General Public License v3.0 or later;
4// see the LICENSE file. There is no warranty, to the extent permitted by law.
5//! Rigid / affine 4x4 transforms.
6
7use nalgebra::{Matrix3, Matrix4, Vector3};
8
9use crate::error::{Error, Result};
10use crate::Point;
11
12/// A 4x4 homogeneous transform (row-major semantics: `p' = M p`).
13///
14/// The usual 4x4: the top-left 3x3 rotates (and, for an affine one, scales or
15/// shears), the last column translates, and the fourth row is `0 0 0 1`. A
16/// point is carried by multiplying on the left, so `a * b` applies `b` first
17/// and then `a` - the order to keep in mind when composing a chain of
18/// registrations.
19///
20/// `Transform(pub Matrix4<f64>)` is a named wrapper around one matrix: a
21/// distinct type, so a transform cannot be mistaken for any other 4x4, with
22/// the matrix itself reachable as `.0` when nalgebra's own operations are
23/// wanted. Being `Copy` means it is duplicated on assignment like a number
24/// rather than moved, which is why transforms are passed around by value
25/// throughout the core.
26#[derive(Debug, Clone, Copy, PartialEq)]
27pub struct Transform(pub Matrix4<f64>);
28
29impl Default for Transform {
30    fn default() -> Self {
31        Transform(Matrix4::identity())
32    }
33}
34
35impl Transform {
36    /// The identity transform, which leaves every point where it is.
37    pub fn identity() -> Self {
38        Self::default()
39    }
40
41    /// From a rotation matrix and translation vector.
42    pub fn from_rt(r: Matrix3<f64>, t: Vector3<f64>) -> Self {
43        let mut m = Matrix4::identity();
44        m.fixed_view_mut::<3, 3>(0, 0).copy_from(&r);
45        m.fixed_view_mut::<3, 1>(0, 3).copy_from(&t);
46        Transform(m)
47    }
48
49    /// From 16 row-major values.
50    pub fn from_row_major(v: &[f64]) -> Result<Self> {
51        if v.len() != 16 {
52            return Err(Error::invalid("transform needs 16 values"));
53        }
54        Ok(Transform(Matrix4::from_row_slice(v)))
55    }
56
57    /// The 16 entries in row-major order, the layout of [`Transform::from_row_major`] and of a RIEGL `.dat` matrix.
58    pub fn to_row_major(&self) -> [f64; 16] {
59        let mut out = [0.0; 16];
60        for r in 0..4 {
61            for c in 0..4 {
62                out[r * 4 + c] = self.0[(r, c)];
63            }
64        }
65        out
66    }
67
68    /// A pure translation by `(dx, dy, dz)` (m).
69    pub fn translation(dx: f64, dy: f64, dz: f64) -> Self {
70        Self::from_rt(Matrix3::identity(), Vector3::new(dx, dy, dz))
71    }
72
73    /// Rotation about z by `deg` degrees.
74    pub fn rotation_z(deg: f64) -> Self {
75        let a = deg.to_radians();
76        let (s, c) = a.sin_cos();
77        Self::from_rt(Matrix3::new(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0), Vector3::zeros())
78    }
79
80    /// RIEGL / RiSCAN convention: `Rz(yaw) Ry(pitch) Rx(roll)` with angles in degrees.
81    pub fn from_roll_pitch_yaw(roll: f64, pitch: f64, yaw: f64) -> Self {
82        let rx = Matrix3::new(
83            1.0, 0.0, 0.0,
84            0.0, roll.to_radians().cos(), -roll.to_radians().sin(),
85            0.0, roll.to_radians().sin(), roll.to_radians().cos(),
86        );
87        let ry = Matrix3::new(
88            pitch.to_radians().cos(), 0.0, pitch.to_radians().sin(),
89            0.0, 1.0, 0.0,
90            -pitch.to_radians().sin(), 0.0, pitch.to_radians().cos(),
91        );
92        let rz = Matrix3::new(
93            yaw.to_radians().cos(), -yaw.to_radians().sin(), 0.0,
94            yaw.to_radians().sin(), yaw.to_radians().cos(), 0.0,
95            0.0, 0.0, 1.0,
96        );
97        Self::from_rt(rz * ry * rx, Vector3::zeros())
98    }
99
100    /// The top-left 3x3 block: the rotation (or, for an affine transform, its linear part).
101    pub fn rotation(&self) -> Matrix3<f64> {
102        self.0.fixed_view::<3, 3>(0, 0).into_owned()
103    }
104
105    /// The translation in the last column (m).
106    pub fn translation_vec(&self) -> Vector3<f64> {
107        self.0.fixed_view::<3, 1>(0, 3).into_owned()
108    }
109
110    #[inline]
111    /// Carry a point: rotate, then translate (m in, m out).
112    pub fn apply(&self, p: &Point) -> Point {
113        let m = &self.0;
114        [
115            m[(0, 0)] * p[0] + m[(0, 1)] * p[1] + m[(0, 2)] * p[2] + m[(0, 3)],
116            m[(1, 0)] * p[0] + m[(1, 1)] * p[1] + m[(1, 2)] * p[2] + m[(1, 3)],
117            m[(2, 0)] * p[0] + m[(2, 1)] * p[1] + m[(2, 2)] * p[2] + m[(2, 3)],
118        ]
119    }
120
121    /// Rotate a direction (no translation).
122    #[inline]
123    pub fn apply_dir(&self, d: &Point) -> Point {
124        let m = &self.0;
125        [
126            m[(0, 0)] * d[0] + m[(0, 1)] * d[1] + m[(0, 2)] * d[2],
127            m[(1, 0)] * d[0] + m[(1, 1)] * d[1] + m[(1, 2)] * d[2],
128            m[(2, 0)] * d[0] + m[(2, 1)] * d[1] + m[(2, 2)] * d[2],
129        ]
130    }
131
132    /// `self ∘ other` (apply `other` first).
133    pub fn compose(&self, other: &Transform) -> Transform {
134        Transform(self.0 * other.0)
135    }
136
137    /// The inverse transform, or an `Invalid` error when the matrix is singular.
138    pub fn inverse(&self) -> Result<Transform> {
139        self.0.try_inverse().map(Transform).ok_or_else(|| Error::invalid("singular transform"))
140    }
141
142    /// Read a whitespace-delimited 4x4 matrix file (RIEGL `.dat` SOP/POP).
143    pub fn read_matrix_file(path: impl AsRef<std::path::Path>) -> Result<Self> {
144        let text = std::fs::read_to_string(path.as_ref()).map_err(|e| Error::file(path.as_ref(), e.to_string()))?;
145        let vals: Vec<f64> = text
146            .split_whitespace()
147            .map(|t| t.parse::<f64>())
148            .collect::<std::result::Result<_, _>>()
149            .map_err(|e| Error::file(path.as_ref(), format!("bad matrix value: {e}")))?;
150        if vals.len() != 16 {
151            return Err(Error::file(path.as_ref(), format!("expected 16 values, got {}", vals.len())));
152        }
153        Self::from_row_major(&vals)
154    }
155}
156
157#[inline]
158pub(crate) fn sub(a: &Point, b: &Point) -> Point {
159    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
160}
161
162#[inline]
163pub(crate) fn dot(a: &Point, b: &Point) -> f64 {
164    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
165}
166
167#[inline]
168pub(crate) fn norm(a: &Point) -> f64 {
169    dot(a, a).sqrt()
170}
171
172#[inline]
173pub(crate) fn scale(a: &Point, s: f64) -> Point {
174    [a[0] * s, a[1] * s, a[2] * s]
175}
176
177#[inline]
178pub(crate) fn add(a: &Point, b: &Point) -> Point {
179    [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
180}
181
182#[inline]
183pub(crate) fn cross(a: &Point, b: &Point) -> Point {
184    [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
185}
186
187#[inline]
188pub(crate) fn normalize(a: &Point) -> Point {
189    let n = norm(a);
190    if n > 0.0 {
191        scale(a, 1.0 / n)
192    } else {
193        [0.0, 0.0, 1.0]
194    }
195}
196
197#[cfg(test)]
198mod tests {
199    use super::*;
200
201    #[test]
202    fn matrix_files_round_trip_and_name_themselves_in_errors() {
203        let dir = std::env::temp_dir().join(format!("sylva-matrix-{}", std::process::id()));
204        std::fs::create_dir_all(&dir).unwrap();
205        let t = Transform::from_roll_pitch_yaw(1.0, -2.0, 30.0).compose(&Transform::translation(10.0, -5.0, 2.5));
206        let text: Vec<String> = t.to_row_major().iter().map(|v| format!("{v:.17e}")).collect();
207        let path = dir.join("sop.dat");
208        std::fs::write(&path, text.chunks(4).map(|r| r.join(" ")).collect::<Vec<_>>().join("\n")).unwrap();
209        assert_eq!(Transform::read_matrix_file(&path).unwrap().to_row_major(), t.to_row_major());
210        let prefix = |p: &std::path::Path| format!("{}: ", p.display());
211        std::fs::write(&path, "1 0 0 0 0 1 0 0 0 0 1 0").unwrap();
212        assert_eq!(Transform::read_matrix_file(&path).unwrap_err().to_string(), format!("{}expected 16 values, got 12", prefix(&path)));
213        std::fs::write(&path, "1 0 0 x").unwrap();
214        assert!(Transform::read_matrix_file(&path).unwrap_err().to_string().starts_with(&format!("{}bad matrix value", prefix(&path))));
215        std::fs::remove_dir_all(&dir).unwrap();
216        assert!(Transform::read_matrix_file(&path).unwrap_err().to_string().starts_with(&prefix(&path)));
217    }
218}