kornia_3d/
linalg.rs

1/// Transform a set of 3D points using a rotation and translation.
2///
3/// # Arguments
4///
5/// * `src_points` - A set of 3D points to be transformed.
6/// * `rotation` - A 3x3 rotation matrix.
7/// * `translation` - A 3D translation vector.
8/// * `dst_points` - A pre-allocated vector to store the transformed 3D points.
9///
10/// PRECONDITION: dst_points is a pre-allocated vector of the same size as source.
11///
12/// Example:
13///
14/// ```
15/// use kornia_3d::linalg::transform_points3d;
16///
17/// let src_points = vec![[2.0, 2.0, 2.0], [3.0, 4.0, 5.0]];
18/// let rotation = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
19/// let translation = [0.0, 0.0, 0.0];
20/// let mut dst_points = vec![[0.0; 3]; src_points.len()];
21/// transform_points3d(&src_points, &rotation, &translation, &mut dst_points);
22/// ```
23pub fn transform_points3d(
24    src_points: &[[f64; 3]],
25    dst_r_src: &[[f64; 3]; 3],
26    dst_t_src: &[f64; 3],
27    dst_points: &mut [[f64; 3]],
28) -> Result<(), Box<dyn std::error::Error>> {
29    if dst_points.len() != src_points.len() {
30        return Err("dst_points must have the same length as src_points".into());
31    }
32
33    for (point_dst, point_src) in dst_points.iter_mut().zip(src_points.iter()) {
34        point_dst[0] = dot_product3(&dst_r_src[0], point_src) + dst_t_src[0];
35        point_dst[1] = dot_product3(&dst_r_src[1], point_src) + dst_t_src[1];
36        point_dst[2] = dot_product3(&dst_r_src[2], point_src) + dst_t_src[2];
37    }
38
39    Ok(())
40}
41
42/// Compute the dot product of two 3D vectors.
43///
44/// # Arguments
45///
46/// * `a` - The first 3D vector.
47/// * `b` - The second 3D vector.
48///
49/// # Returns
50///
51/// The dot product of the two vectors.
52///
53/// # Panics
54///
55/// Panics if the vectors are not of the same length or if the length is not 3.
56///
57/// # Example
58///
59/// ```
60/// use kornia_3d::linalg::dot_product3;
61///
62/// let a = [1.0, 2.0, 3.0];
63/// let b = [4.0, 5.0, 6.0];
64/// let result = dot_product3(&a, &b);
65/// assert_eq!(result, 32.0);
66/// ```
67pub fn dot_product3(a: &[f64; 3], b: &[f64; 3]) -> f64 {
68    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
69}
70
71/// Multiply two 3x3 matrices.
72///
73/// # Arguments
74///
75/// * `a` - The left hand side 3x3 matrix.
76/// * `b` - The right hand side 3x3 matrix.
77/// * `m` - A pre-allocated 3x3 matrix to store the result.
78///
79/// PRECONDITION: m is a pre-allocated 3x3 matrix.
80///
81/// # Example
82///
83/// ```
84/// use kornia_3d::linalg::matmul33;
85///
86/// let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
87/// let b = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
88/// let mut m = [[0.0; 3]; 3];
89/// matmul33(&a, &b, &mut m);
90/// ```
91pub fn matmul33(a: &[[f64; 3]; 3], b: &[[f64; 3]; 3], m: &mut [[f64; 3]; 3]) {
92    m[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0];
93    m[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1];
94    m[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2];
95
96    m[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0];
97    m[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1];
98    m[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2];
99
100    m[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0];
101    m[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1];
102    m[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2];
103}
104
105/// Transpose a 3x3 matrix.
106///
107/// # Arguments
108///
109/// * `a` - The 3x3 matrix to transpose.
110/// * `m` - A pre-allocated 3x3 matrix to store the result.
111///
112/// PRECONDITION: m is a pre-allocated 3x3 matrix.
113///
114/// # Example
115///
116/// ```
117/// use kornia_3d::linalg::transpose_mat33;
118///
119/// let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
120/// let mut m = [[0.0; 3]; 3];
121/// transpose_mat33(&a, &mut m);
122/// ```
123pub fn transpose_mat33(a: &[[f64; 3]; 3], m: &mut [[f64; 3]; 3]) {
124    m[0] = [a[0][0], a[1][0], a[2][0]]; // First column
125    m[1] = [a[0][1], a[1][1], a[2][1]]; // Second column
126    m[2] = [a[0][2], a[1][2], a[2][2]]; // Third column
127}
128
129/// Transpose a 3x3 matrix in place.
130///
131/// # Arguments
132///
133/// * `a` - The 3x3 matrix to transpose.
134///
135/// # Example
136///
137/// ```
138/// use kornia_3d::linalg::transpose_mat33_inplace;
139///
140/// let mut a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
141/// transpose_mat33_inplace(&mut a);
142/// ```
143pub fn transpose_mat33_inplace(a: &mut [[f64; 3]; 3]) {
144    let a01 = a[0][1];
145    let a02 = a[0][2];
146    let a12 = a[1][2];
147
148    a[0][1] = a[1][0];
149    a[0][2] = a[2][0];
150    a[1][2] = a[2][1];
151    a[1][0] = a01;
152    a[2][0] = a02;
153    a[2][1] = a12;
154}
155
156/// Multiply a 3x3 matrix by a 3D vector.
157///
158/// # Arguments
159///
160/// * `a` - The 3x3 matrix.
161/// * `b` - The 3D vector.
162/// * `m` - A pre-allocated 3D vector to store the result.
163///
164/// PRECONDITION: m is a pre-allocated 3D vector.
165///
166/// # Example
167///
168/// ```
169/// use kornia_3d::linalg::mat33_mul_vec3;
170///
171/// let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
172/// let b = [1.0, 2.0, 3.0];
173/// let mut m = [0.0; 3];
174/// mat33_mul_vec3(&a, &b, &mut m);
175/// ```
176pub fn mat33_mul_vec3(a: &[[f64; 3]; 3], b: &[f64; 3], m: &mut [f64; 3]) {
177    m[0] = dot_product3(&a[0], b);
178    m[1] = dot_product3(&a[1], b);
179    m[2] = dot_product3(&a[2], b);
180}
181
182/// Compute the Frobenius norm of a 3x3 matrix.
183///
184/// # Arguments
185///
186/// * `m` - The 3x3 matrix.
187///
188/// # Returns
189///
190/// The Frobenius norm of the matrix.
191///
192/// # Example
193///
194/// ```
195/// use kornia_3d::linalg::frobenius_norm33;
196///
197/// let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
198/// let result = frobenius_norm33(&a);
199/// assert_eq!(result, 285.0_f64.sqrt());
200/// ```
201pub fn frobenius_norm33(m: &[[f64; 3]; 3]) -> f64 {
202    m.iter().flatten().map(|x| x * x).sum::<f64>().sqrt()
203}
204
205/// Divide a 3x3 matrix by a scalar in place.
206///
207/// # Arguments
208///
209/// * `m` - The 3x3 matrix to divide.
210/// * `scalar` - The scalar to divide the matrix by.
211///
212/// # Example
213///
214/// ```
215/// use kornia_3d::linalg::mat33_div_scalar_inplace;
216///
217/// let mut a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
218/// mat33_div_scalar_inplace(&mut a, 2.0);
219/// ```
220pub fn mat33_div_scalar_inplace(m: &mut [[f64; 3]; 3], scalar: f64) {
221    m.iter_mut().flatten().for_each(|x| *x /= scalar);
222}
223
224/// Compute the determinant of a 3x3 matrix.
225///
226/// # Arguments
227///
228/// * `m` - The 3x3 matrix.
229///
230/// # Returns
231///
232/// The determinant of the matrix.
233///
234/// # Example
235///
236/// ```
237/// use kornia_3d::linalg::det_mat33;
238///
239/// let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
240/// let result = det_mat33(&a);
241/// assert_eq!(result, 0.0);
242/// ```
243pub fn det_mat33(m: &[[f64; 3]; 3]) -> f64 {
244    m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
245        - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
246        + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])
247}
248
249/// Compute the cross product of two 3D vectors.
250///
251/// # Arguments
252///
253/// * `a` - The first 3D vector.
254/// * `b` - The second 3D vector.
255/// * `c` - A pre-allocated 3D vector to store the result.
256///
257/// PRECONDITION: c is a pre-allocated 3D vector.
258///
259/// # Example
260///
261/// ```
262/// use kornia_3d::linalg::cross_vec3;
263///
264/// let a = [1.0, 2.0, 3.0];
265/// let b = [4.0, 5.0, 6.0];
266/// let mut c = [0.0; 3];
267/// cross_vec3(&a, &b, &mut c);
268/// ```
269pub fn cross_vec3(a: &[f64; 3], b: &[f64; 3], c: &mut [f64; 3]) {
270    c[0] = a[1] * b[2] - a[2] * b[1]; // x-component
271    c[1] = a[2] * b[0] - a[0] * b[2]; // y-component
272    c[2] = a[0] * b[1] - a[1] * b[0]; // z-component
273}
274
275/// Normalize a 3x3 matrix in place.
276///
277/// # Arguments
278///
279/// * `m` - The 3x3 matrix to normalize.
280///
281/// # Example
282///
283/// ```
284/// use kornia_3d::linalg::normalize_mat33_inplace;
285///
286/// let mut a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
287/// normalize_mat33_inplace(&mut a);
288/// ```
289pub fn normalize_mat33_inplace(m: &mut [[f64; 3]; 3]) {
290    let norm = m[2][2];
291    mat33_div_scalar_inplace(m, norm);
292}
293
294#[cfg(test)]
295mod tests {
296    use super::*;
297
298    #[test]
299    fn test_dot_product3() {
300        let a = [1.0, 2.0, 3.0];
301        let b = [4.0, 5.0, 6.0];
302        let result = dot_product3(&a, &b);
303        assert_eq!(result, 32.0);
304    }
305
306    #[test]
307    fn test_matmul33() {
308        let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
309        let b = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
310        let mut m = [[0.0; 3]; 3];
311        matmul33(&a, &b, &mut m);
312        assert_eq!(
313            m,
314            [
315                [30.0, 36.0, 42.0],
316                [66.0, 81.0, 96.0],
317                [102.0, 126.0, 150.0]
318            ]
319        );
320    }
321
322    #[test]
323    fn test_transpose_mat33() {
324        let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
325        let mut m = [[0.0; 3]; 3];
326        transpose_mat33(&a, &mut m);
327        assert_eq!(m, [[1.0, 4.0, 7.0], [2.0, 5.0, 8.0], [3.0, 6.0, 9.0]]);
328    }
329
330    #[test]
331    fn test_transpose_mat33_inplace() {
332        let mut a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
333        transpose_mat33_inplace(&mut a);
334        assert_eq!(a, [[1.0, 4.0, 7.0], [2.0, 5.0, 8.0], [3.0, 6.0, 9.0]]);
335    }
336
337    #[test]
338    fn test_mat33_mul_vec3() {
339        let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
340        let b = [1.0, 2.0, 3.0];
341        let mut m = [0.0; 3];
342        mat33_mul_vec3(&a, &b, &mut m);
343        assert_eq!(m, [14.0, 32.0, 50.0]);
344    }
345
346    #[test]
347    fn test_mat33_div_scalar_inplace() {
348        let mut a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
349        mat33_div_scalar_inplace(&mut a, 2.0);
350        assert_eq!(a, [[0.5, 1.0, 1.5], [2.0, 2.5, 3.0], [3.5, 4.0, 4.5]]);
351    }
352
353    #[test]
354    fn test_det_mat33() {
355        let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
356        let result = det_mat33(&a);
357        assert_eq!(result, 0.0);
358    }
359
360    #[test]
361    fn test_frobenius_norm33() {
362        let a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
363        let result = frobenius_norm33(&a);
364        assert_eq!(result, 285.0_f64.sqrt());
365    }
366
367    #[test]
368    fn test_cross_vec3() {
369        let a = [1.0, 2.0, 3.0];
370        let b = [4.0, 5.0, 6.0];
371        let mut c = [0.0; 3];
372        cross_vec3(&a, &b, &mut c);
373        assert_eq!(c, [-3.0, 6.0, -3.0]);
374    }
375
376    #[test]
377    fn test_normalize_mat33_inplace() {
378        let mut a = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
379        normalize_mat33_inplace(&mut a);
380        assert_eq!(
381            a,
382            [
383                [1.0 / 9.0, 2.0 / 9.0, 3.0 / 9.0],
384                [4.0 / 9.0, 5.0 / 9.0, 6.0 / 9.0],
385                [7.0 / 9.0, 8.0 / 9.0, 9.0 / 9.0]
386            ]
387        );
388    }
389
390    #[test]
391    fn test_transform_points_identity() -> Result<(), Box<dyn std::error::Error>> {
392        let src_points = vec![[2.0, 2.0, 2.0], [3.0, 4.0, 5.0]];
393        let rotation = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
394        let translation = [0.0, 0.0, 0.0];
395        let mut dst_points = vec![[0.0; 3]; src_points.len()];
396        transform_points3d(&src_points, &rotation, &translation, &mut dst_points)?;
397
398        assert_eq!(dst_points, src_points);
399
400        Ok(())
401    }
402
403    #[test]
404    fn test_transform_points_roundtrip() -> Result<(), Box<dyn std::error::Error>> {
405        let src_points = vec![[2.0, 2.0, 2.0], [3.0, 4.0, 5.0]];
406        let rotation = [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]];
407        let translation = [1.0, 2.0, 3.0];
408
409        let mut dst_points = vec![[0.0; 3]; src_points.len()];
410        transform_points3d(&src_points, &rotation, &translation, &mut dst_points)?;
411
412        // invert the transformation
413        let dst_r_src = {
414            let rotation_slice = unsafe {
415                std::slice::from_raw_parts(rotation.as_ptr() as *const f64, rotation.len() * 3)
416            };
417            faer::mat::from_row_major_slice(rotation_slice, 3, 3)
418        };
419        let dst_t_src = faer::col![translation[0], translation[1], translation[2]];
420        // R' = R^T
421        let src_r_dst = dst_r_src.transpose();
422        // t' = -R^T * t
423        let src_t_dst = -src_r_dst * dst_t_src;
424        let (rotation_inv, translation_inv) = {
425            let mut rotation_inv = [[0.0; 3]; 3];
426            for (i, row) in rotation_inv.iter_mut().enumerate() {
427                for (j, val) in row.iter_mut().enumerate() {
428                    *val = src_r_dst.read(i, j);
429                }
430            }
431            let mut translation_inv = [0.0; 3];
432            for (i, val) in translation_inv.iter_mut().enumerate() {
433                *val = src_t_dst.read(i);
434            }
435            (rotation_inv, translation_inv)
436        };
437
438        // transform dst_points back to src_points
439        let mut dst_points_src = vec![[0.0; 3]; dst_points.len()];
440        transform_points3d(
441            &dst_points,
442            &rotation_inv,
443            &translation_inv,
444            &mut dst_points_src,
445        )?;
446
447        assert_eq!(dst_points_src, src_points);
448
449        Ok(())
450    }
451}
kornia_3d/linalg.rs

kornia_3d/
linalg.rs
