cu-spatial-payloads 1.0.0-rc1

Spatial payloads for the Copper.
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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112

use alloc::vec::Vec;
use core::fmt::Debug;

use rerun::components::TransformMat3x3;
use rerun::datatypes::Mat3x3;
use rerun::{AsComponents, GeoPoints, SerializedComponentBatch, Transform3D as RerunTransform3D};

use crate::{GeodeticPosition, Transform3D};

fn transform_parts<T: Copy + Into<f64> + Debug + Default + 'static>(
    transform: &Transform3D<T>,
) -> ([f32; 3], [[f32; 3]; 3]) {
    let matrix = (*transform).to_matrix();
    let translation = [
        matrix[3][0].into() as f32,
        matrix[3][1].into() as f32,
        matrix[3][2].into() as f32,
    ];
    let mat3 = [
        [
            matrix[0][0].into() as f32,
            matrix[0][1].into() as f32,
            matrix[0][2].into() as f32,
        ],
        [
            matrix[1][0].into() as f32,
            matrix[1][1].into() as f32,
            matrix[1][2].into() as f32,
        ],
        [
            matrix[2][0].into() as f32,
            matrix[2][1].into() as f32,
            matrix[2][2].into() as f32,
        ],
    ];

    (translation, mat3)
}

fn build_rerun_transform<T: Copy + Into<f64> + Debug + Default + 'static>(
    transform: &Transform3D<T>,
) -> RerunTransform3D {
    let (translation, mat3) = transform_parts(transform);
    let mat_flat = [
        mat3[0][0], mat3[0][1], mat3[0][2], mat3[1][0], mat3[1][1], mat3[1][2], mat3[2][0],
        mat3[2][1], mat3[2][2],
    ];

    RerunTransform3D::new()
        .with_translation(translation)
        .with_mat3x3(TransformMat3x3::from(Mat3x3(mat_flat)))
}

fn build_rerun_geo_point(position: &GeodeticPosition) -> GeoPoints {
    GeoPoints::from_lat_lon([(position.latitude_degrees(), position.longitude_degrees())])
}

impl AsComponents for GeodeticPosition {
    fn as_serialized_batches(&self) -> Vec<SerializedComponentBatch> {
        build_rerun_geo_point(self).as_serialized_batches()
    }
}

impl AsComponents for Transform3D<f32> {
    fn as_serialized_batches(&self) -> Vec<SerializedComponentBatch> {
        build_rerun_transform(self).as_serialized_batches()
    }
}

impl AsComponents for Transform3D<f64> {
    fn as_serialized_batches(&self) -> Vec<SerializedComponentBatch> {
        build_rerun_transform(self).as_serialized_batches()
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::GeodeticPosition;

    #[test]
    fn transform_parts_uses_fourth_row_for_translation() {
        let transform = Transform3D::<f32>::from_matrix([
            [1.0, 0.0, 0.0, 0.0],
            [0.0, 1.0, 0.0, 0.0],
            [0.0, 0.0, 1.0, 0.0],
            [10.0, 20.0, 30.0, 1.0],
        ]);

        let (translation, _mat3) = transform_parts(&transform);
        assert_eq!(translation, [10.0, 20.0, 30.0]);
    }

    #[test]
    fn transform3d_emits_rerun_components() {
        let transform = Transform3D::<f32>::from_matrix([
            [1.0, 0.0, 0.0, 0.1],
            [0.0, 1.0, 0.0, 0.2],
            [0.0, 0.0, 1.0, 0.3],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        assert!(!transform.as_serialized_batches().is_empty());
    }

    #[test]
    fn geodetic_position_emits_rerun_components() {
        let position = GeodeticPosition::from_degrees(59.319_221, 18.075_631);

        assert!(!position.as_serialized_batches().is_empty());
    }
}