1use nalgebra::{Matrix3, Matrix4, Vector3};
8
9use crate::error::{Error, Result};
10use crate::Point;
11
12#[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 pub fn identity() -> Self {
38 Self::default()
39 }
40
41 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 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 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 pub fn translation(dx: f64, dy: f64, dz: f64) -> Self {
70 Self::from_rt(Matrix3::identity(), Vector3::new(dx, dy, dz))
71 }
72
73 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 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 pub fn rotation(&self) -> Matrix3<f64> {
102 self.0.fixed_view::<3, 3>(0, 0).into_owned()
103 }
104
105 pub fn translation_vec(&self) -> Vector3<f64> {
107 self.0.fixed_view::<3, 1>(0, 3).into_owned()
108 }
109
110 #[inline]
111 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 #[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 pub fn compose(&self, other: &Transform) -> Transform {
134 Transform(self.0 * other.0)
135 }
136
137 pub fn inverse(&self) -> Result<Transform> {
139 self.0.try_inverse().map(Transform).ok_or_else(|| Error::invalid("singular transform"))
140 }
141
142 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}