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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161

//! Defines the profile type of streams.

use crate::{
    base::{Resolution, StreamProfileData},
    error::{ErrorChecker, Result as RsResult},
    kind::{Extension, Format, StreamKind},
};
use nalgebra::{Isometry3, MatrixMN, Translation3, UnitQuaternion, U3};
use num_traits::FromPrimitive;
use std::{borrow::Borrow, mem::MaybeUninit, ptr::NonNull};

/// The profile of stream.
#[derive(Debug)]
pub struct StreamProfile {
    ptr: NonNull<realsense_sys::rs2_stream_profile>,
    from_clone: bool,
}

impl StreamProfile {
    /// Check whether the profile is default or not.
    pub fn is_default(&self) -> RsResult<bool> {
        unsafe {
            let mut checker = ErrorChecker::new();
            let val = realsense_sys::rs2_is_stream_profile_default(
                self.ptr.as_ptr(),
                checker.inner_mut_ptr(),
            );
            checker.check()?;
            Ok(val != 0)
        }
    }

    /// Gets the resolution of stream.
    pub fn resolution(&self) -> RsResult<Resolution> {
        let mut width = MaybeUninit::uninit();
        let mut height = MaybeUninit::uninit();
        let resolution = unsafe {
            let mut checker = ErrorChecker::new();
            realsense_sys::rs2_get_video_stream_resolution(
                self.ptr.as_ptr(),
                width.as_mut_ptr(),
                height.as_mut_ptr(),
                checker.inner_mut_ptr(),
            );
            checker.check()?;
            let resolution = Resolution {
                width: width.assume_init() as usize,
                height: height.assume_init() as usize,
            };
            resolution
        };
        Ok(resolution)
    }

    /// Check if the stream is extendable to the given extension.
    pub fn is_extendable_to(&mut self, extension: Extension) -> RsResult<()> {
        unsafe {
            let mut checker = ErrorChecker::new();
            realsense_sys::rs2_stream_profile_is(
                self.ptr.as_ptr(),
                extension as realsense_sys::rs2_extension,
                checker.inner_mut_ptr(),
            );
            checker.check()?;
        }
        Ok(())
    }

    /// Gets the attributes of stream.
    pub fn get_data(&self) -> RsResult<StreamProfileData> {
        unsafe {
            let mut checker = ErrorChecker::new();
            let mut stream = MaybeUninit::uninit();
            let mut format = MaybeUninit::uninit();
            let mut index = MaybeUninit::uninit();
            let mut unique_id = MaybeUninit::uninit();
            let mut framerate = MaybeUninit::uninit();

            realsense_sys::rs2_get_stream_profile_data(
                self.ptr.as_ptr(),
                stream.as_mut_ptr(),
                format.as_mut_ptr(),
                index.as_mut_ptr(),
                unique_id.as_mut_ptr(),
                framerate.as_mut_ptr(),
                checker.inner_mut_ptr(),
            );
            checker.check()?;

            let data = StreamProfileData {
                stream: StreamKind::from_u32(stream.assume_init()).unwrap(),
                format: Format::from_u32(format.assume_init()).unwrap(),
                index: index.assume_init() as usize,
                unique_id: unique_id.assume_init(),
                framerate: framerate.assume_init(),
            };
            Ok(data)
        }
    }

    // /// Sets the attributes of stream.
    // pub fn set_data(&mut self) {
    //     todo!();
    // }

    /// Compute the extrinsic parameters to another stream.
    pub fn extrinsics_to<P>(&self, other: P) -> RsResult<Isometry3<f32>>
    where
        P: Borrow<StreamProfile>,
    {
        let mut extrinsics = MaybeUninit::<realsense_sys::rs2_extrinsics>::uninit();
        let (raw_rotation, raw_translation) = unsafe {
            let mut checker = ErrorChecker::new();
            realsense_sys::rs2_get_extrinsics(
                self.ptr.as_ptr(),
                other.borrow().ptr.as_ptr(),
                extrinsics.as_mut_ptr(),
                checker.inner_mut_ptr(),
            );
            checker.check()?;

            let realsense_sys::rs2_extrinsics {
                rotation,
                translation,
            } = extrinsics.assume_init();
            (rotation, translation)
        };

        let rotation = {
            let matrix =
                MatrixMN::<f32, U3, U3>::from_iterator(raw_rotation.iter().map(|val| *val));
            UnitQuaternion::from_matrix(&matrix)
        };

        let translation =
            { Translation3::new(raw_translation[0], raw_translation[1], raw_translation[2]) };

        let isometry = Isometry3::from_parts(translation, rotation);

        Ok(isometry)
    }

    pub(crate) unsafe fn from_parts(
        ptr: NonNull<realsense_sys::rs2_stream_profile>,
        from_clone: bool,
    ) -> Self {
        Self { ptr, from_clone }
    }
}

impl Drop for StreamProfile {
    fn drop(&mut self) {
        unsafe {
            if self.from_clone {
                realsense_sys::rs2_delete_stream_profile(self.ptr.as_ptr());
            }
        }
    }
}

unsafe impl Send for StreamProfile {}