kornia-3d 0.1.14

3d point cloud processing library
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

/// Compute the rotation matrix from an axis and angle.
///
/// # Arguments
///
/// * `axis` - The axis of rotation.
/// * `angle` - The angle of rotation.
///
/// # Returns
///
/// The rotation matrix.
///
/// PRECONDITION: axis is a unit vector.
///
/// Example:
///
/// ```no_run
/// use kornia_3d::transforms::axis_angle_to_rotation_matrix;
///
/// let axis = [1.0, 0.0, 0.0];
/// let angle = std::f64::consts::PI / 2.0;
/// let rotation = axis_angle_to_rotation_matrix(&axis, angle).unwrap();
/// assert_eq!(rotation, [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]);
/// ```
pub fn axis_angle_to_rotation_matrix(
    axis: &[f64; 3],
    angle: f64,
) -> Result<[[f64; 3]; 3], &'static str> {
    // normalize the vector
    let axis_norm = {
        let magnitude = (axis[0].powi(2) + axis[1].powi(2) + axis[2].powi(2)).sqrt();
        match magnitude < 1e-10 {
            true => return Err("cannot compute rotation matrix from a zero vector"),
            false => [
                axis[0] / magnitude,
                axis[1] / magnitude,
                axis[2] / magnitude,
            ],
        }
    };

    let x = axis_norm[0];
    let y = axis_norm[1];
    let z = axis_norm[2];

    let c = angle.cos();
    let s = angle.sin();
    let t = 1.0 - c;

    let m00 = c + x * x * t;
    let m11 = c + y * y * t;
    let m22 = c + z * z * t;

    let tmp1 = x * y * t;
    let tmp2 = z * s;

    let m10 = tmp1 + tmp2;
    let m01 = tmp1 - tmp2;

    let tmp3 = x * z * t;
    let tmp4 = y * s;

    let m20 = tmp3 - tmp4;
    let m02 = tmp3 + tmp4;

    let tmp5 = y * z * t;
    let tmp6 = x * s;

    let m12 = tmp5 - tmp6;
    let m21 = tmp5 + tmp6;

    Ok([[m00, m01, m02], [m10, m11, m12], [m20, m21, m22]])
}

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;

    #[test]
    fn test_axis_angle_to_rotation_matrix_identity() -> Result<(), Box<dyn std::error::Error>> {
        let axis = [1.0, 0.0, 0.0];
        let angle = std::f64::consts::PI / 2.0;
        let rotation = axis_angle_to_rotation_matrix(&axis, angle)?;
        let expected = [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]];
        for i in 0..3 {
            for j in 0..3 {
                assert_relative_eq!(rotation[i][j], expected[i][j]);
            }
        }
        Ok(())
    }
}