polyhorn_ui/linalg/
transform.rs

1use num_traits::Float;
2
3use super::{Point3D, Quaternion3D};
4
5/// Rank 4 row-major transformation matrix for 3D objects.
6#[derive(Copy, Clone, Debug)]
7pub struct Transform3D<T> {
8    /// This field contains the entries of this matrix in column-major order.
9    pub columns: [[T; 4]; 4],
10}
11
12impl<T> Transform3D<T> {
13    /// Returns a new transformation matrix with the given entries in row-major
14    /// order. This function does not perform any kind of validation on the
15    /// given entries. It is the caller's responsibility to ensure that the
16    /// given entries result in a valid transformation matrix.
17    pub fn new(columns: [[T; 4]; 4]) -> Transform3D<T> {
18        Transform3D { columns }
19    }
20
21    /// Applies the given transformation to each entry in the transformation
22    /// matrix individually.
23    pub fn map<F, O>(self, mut op: F) -> Transform3D<O>
24    where
25        F: FnMut(T) -> O,
26    {
27        let [[a00, a01, a02, a03], [a10, a11, a12, a13], [a20, a21, a22, a23], [a30, a31, a32, a33]] =
28            self.columns;
29
30        Transform3D {
31            columns: [
32                [op(a00), op(a01), op(a02), op(a03)],
33                [op(a10), op(a11), op(a12), op(a13)],
34                [op(a20), op(a21), op(a22), op(a23)],
35                [op(a30), op(a31), op(a32), op(a33)],
36            ],
37        }
38    }
39}
40
41impl<T> Transform3D<T>
42where
43    T: Float,
44{
45    /// Returns a new identity matrix, i.e. a matrix of zeros with ones on the
46    /// diagonal.
47    pub fn identity() -> Transform3D<T> {
48        let i = T::one();
49        let o = T::zero();
50
51        Transform3D {
52            columns: [[i, o, o, o], [o, i, o, o], [o, o, i, o], [o, o, o, i]],
53        }
54    }
55
56    /// Returns a new transformation matrix for a perspective with the given
57    /// depth.
58    pub fn with_perspective(d: T) -> Transform3D<T> {
59        let mut columns = Self::identity().columns;
60        columns[2][3] = d.recip();
61        Transform3D { columns }
62    }
63
64    /// Returns a new transformation matrix for a translation with the given
65    /// offsets.
66    pub fn with_translation(tx: T, ty: T, tz: T) -> Transform3D<T> {
67        let mut columns = Self::identity().columns;
68        columns[3][0] = tx;
69        columns[3][1] = ty;
70        columns[3][2] = tz;
71        Transform3D { columns }
72    }
73
74    /// Returns a new transformation matrix for a scale with the given factors.
75    pub fn with_scale(sx: T, sy: T, sz: T) -> Transform3D<T> {
76        let mut columns = Self::identity().columns;
77        columns[0][0] = sx;
78        columns[1][1] = sy;
79        columns[2][2] = sz;
80        Transform3D { columns }
81    }
82
83    /// Returns a new transformation matrix for a horizontal skew with the
84    /// given angle radians.
85    pub fn with_skew_x(sx: T) -> Transform3D<T> {
86        let mut columns = Self::identity().columns;
87        columns[1][0] = sx.tan();
88        Transform3D { columns }
89    }
90
91    /// Returns a new transformation matrix for a vertical skew with the given
92    /// angle radians.
93    pub fn with_skew_y(sy: T) -> Transform3D<T> {
94        let mut columns = Self::identity().columns;
95        columns[0][1] = sy.tan();
96        Transform3D { columns }
97    }
98
99    /// Returns a new transformation matrix for a counter-clockwise rotation
100    /// with the given angle radians around the given vector. The given vector
101    /// does not have to be a unit vector. If it is not a unit vector, this
102    /// function will normalize and and multiply the angle with the original
103    /// norm.
104    pub fn with_rotation(q: Quaternion3D<T>) -> Transform3D<T> {
105        let Quaternion3D { x, y, z, w } = q;
106
107        let one = T::one();
108        let two = one + one;
109
110        let mut columns = Self::identity().columns;
111        columns[0][0] = one - two * (y * y + z * z);
112        columns[0][1] = two * (x * y + z * w);
113        columns[0][2] = two * (x * z - y * w);
114        columns[1][0] = two * (x * y - z * w);
115        columns[1][1] = one - two * (x * x + z * z);
116        columns[1][2] = two * (y * z + x * w);
117        columns[2][0] = two * (x * z + y * w);
118        columns[2][1] = two * (y * z - x * w);
119        columns[2][2] = one - two * (x * x + y * y);
120
121        Transform3D { columns }
122    }
123
124    /// Translates the given transformation matrix with the given offsets by
125    /// concatenating the given matrix to a new translation matrix.
126    pub fn translate(self, tx: T, ty: T, tz: T) -> Transform3D<T> {
127        self.concat(Self::with_translation(tx, ty, tz))
128    }
129
130    /// Scales the given transformation matrix with the given factors by
131    /// concatenating the given matrix to a new scale matrix.
132    pub fn scale(self, sx: T, sy: T, sz: T) -> Transform3D<T> {
133        self.concat(Self::with_scale(sx, sy, sz))
134    }
135
136    /// Rotates the given transformation matrix with the given angle in radians
137    /// around the given vector. The rotation is counter-clockwise and if the
138    /// given vector is not unit, it will be normalized and the angle will be
139    /// multiplied with the length of the original vector.
140    pub fn rotate(self, q: Quaternion3D<T>) -> Transform3D<T> {
141        self.concat(Self::with_rotation(q))
142    }
143
144    /// Concatenates the given transformation matrix to the current
145    /// transformation matrix.
146    pub fn concat(self, other: Transform3D<T>) -> Transform3D<T> {
147        macro_rules! dot {
148            ($c:expr, $a:expr, $b:expr, $i:literal, $j:literal) => {
149                $c[$i][$j] = $a[$i][0] * $b[0][$j]
150                    + $a[$i][1] * $b[1][$j]
151                    + $a[$i][2] * $b[2][$j]
152                    + $a[$i][3] * $b[3][$j];
153            };
154        }
155
156        let mut columns = Self::identity().columns;
157        dot!(columns, other.columns, self.columns, 0, 0);
158        dot!(columns, other.columns, self.columns, 0, 1);
159        dot!(columns, other.columns, self.columns, 0, 2);
160        dot!(columns, other.columns, self.columns, 0, 3);
161        dot!(columns, other.columns, self.columns, 1, 0);
162        dot!(columns, other.columns, self.columns, 1, 1);
163        dot!(columns, other.columns, self.columns, 1, 2);
164        dot!(columns, other.columns, self.columns, 1, 3);
165        dot!(columns, other.columns, self.columns, 2, 0);
166        dot!(columns, other.columns, self.columns, 2, 1);
167        dot!(columns, other.columns, self.columns, 2, 2);
168        dot!(columns, other.columns, self.columns, 2, 3);
169        dot!(columns, other.columns, self.columns, 3, 0);
170        dot!(columns, other.columns, self.columns, 3, 1);
171        dot!(columns, other.columns, self.columns, 3, 2);
172        dot!(columns, other.columns, self.columns, 3, 3);
173
174        Transform3D { columns }
175    }
176
177    /// Applies the given transformation matrix to a point.
178    pub fn apply(self, point: Point3D<T>) -> Point3D<T> {
179        let x = self.columns[0][0] * point.x
180            + self.columns[1][0] * point.y
181            + self.columns[2][0] * point.z
182            + self.columns[3][0];
183
184        let y = self.columns[0][1] * point.x
185            + self.columns[1][1] * point.y
186            + self.columns[2][1] * point.z
187            + self.columns[3][1];
188
189        let z = self.columns[0][2] * point.x
190            + self.columns[1][2] * point.y
191            + self.columns[2][2] * point.z
192            + self.columns[3][2];
193
194        let q = self.columns[0][3] * point.x
195            + self.columns[1][3] * point.y
196            + self.columns[2][3] * point.z
197            + self.columns[3][3];
198
199        Point3D {
200            x: x / q,
201            y: y / q,
202            z: z / q,
203        }
204    }
205
206    /// Multiplies every element of this transformation with the given scalar
207    /// and returns the result.
208    pub fn multiply_scalar(mut self, scalar: T) -> Transform3D<T> {
209        for i in 0..4 {
210            for j in 0..4 {
211                self.columns[i][j] = self.columns[i][j] * scalar;
212            }
213        }
214
215        self
216    }
217
218    fn subtract(self, other: Transform3D<T>) -> Transform3D<T> {
219        let mut columns = self.columns;
220        for i in 0..4 {
221            for j in 0..4 {
222                columns[i][j] = self.columns[i][j] - other.columns[i][j];
223            }
224        }
225        Transform3D { columns }
226    }
227
228    fn l2_norm(self) -> T {
229        (0..4).into_iter().fold(T::zero(), |acc, i| {
230            acc + (0..4).into_iter().fold(T::zero(), |acc, j| {
231                acc + self.columns[i][j] * self.columns[i][j]
232            })
233        })
234    }
235}
236
237impl<T> Default for Transform3D<T>
238where
239    T: Float,
240{
241    fn default() -> Self {
242        Transform3D::identity()
243    }
244}
245
246impl<T> Eq for Transform3D<T> where T: Float {}
247
248impl<T> PartialEq for Transform3D<T>
249where
250    T: Float,
251{
252    fn eq(&self, other: &Transform3D<T>) -> bool {
253        self.subtract(*other).l2_norm() < T::from(1.0 / 1000.0).unwrap()
254    }
255}
polyhorn_ui/linalg/transform.rs

polyhorn_ui/linalg/
transform.rs
