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

use snafu::Snafu;

#[cfg(target_os = "linux")]
/// Implementations of [`HasViconHardware`]
/// for use with a real Vicon system.
pub mod sys;

/// A thing that can read from a Vicon data stream.
pub trait HasViconHardware {
    /// Returns a list of all identified [`ViconSubject`]s
    /// in the next available frame from the system.
    fn read_frame_subjects(&mut self) -> Result<Vec<ViconSubject>, ViconError>;
}

/// A single subject identified in a frame
/// read by a thing that [`HasViconHardware`].
#[derive(Debug)]
pub struct ViconSubject {
    /// The subject's name.
    pub name: String,

    /// The subject's in cartesian space.
    pub origin: Vector3D,

    /// TODO: Should the subject instead
    ///       have its quaternion instead
    ///       of these values?
    ///
    /// The subject's yaw, pitch, and roll
    /// in space, derived from its rotational
    /// quaternion.
    ///
    /// The axes correspond to the subject's
    /// yaw ([`Vector3D::x`]), pitch ([`Vector3D::y`]),
    /// and roll ([`Vector3D::z`]),
    pub rotation: Vector3D,
}

/// A three-dimensional vector of data.
#[derive(Debug)]
pub struct Vector3D {
    /// The X-axis (1st dimension).
    pub x: f64,

    /// The Y-axis (2nd dimension).
    pub y: f64,

    /// The Z-axis (3rd dimension).
    pub z: f64,
}

/// Enumeration of errors returned by a
/// thing that [HasViconHardware].
#[derive(Debug, Snafu)]
pub enum ViconError {
    /// An error from the Vicon SDK.
    SdkError { source: ViconSdkStatus },
}

/// Implementation of [`TryFrom`] which
/// returns `Ok` for _successful_
/// [`ViconSdkStatus`] codes, and `Err`
/// for all other status codes.
impl TryFrom<i32> for ViconError {
    type Error = ViconError;

    fn try_from(value: i32) -> Result<Self, Self::Error> {
        let status = ViconSdkStatus::from(value);

        if status.is_success() {
            Ok(ViconError::SdkError { source: status })
        } else {
            Err(ViconError::SdkError { source: status })
        }
    }
}

/// Enumeration of status codes returned
/// by Vicon data stream SDK.
///
/// These status codes are derived from
/// the codes listed in the Vicon SDK's
/// `CTypeDefs.h` file.
#[derive(Debug, Snafu)]
pub enum ViconSdkStatus {
    Unknown { code: i32 },
    Unimplemented,
    Success,
    InvalidHostname,
    InvalidMulticastIp,
    ClientAlreadyConnected,
    ClientConnectionFailed,
    ServerAlreadyTransmittingMulticast,
    ServerNotTransmittingMulticast,
    NotConnected,
    NoDataFrame,
    InvalidIndex,
    InvalidCameraName,
    InvalidSubjectName,
    InvalidSegmentName,
    InvalidMarkerName,
    InvalidDeviceName,
    InvalidDeviceOutputName,
    InvalidLatencySampleRate,
    InvalidCoLinearAxes,
    LeftHandedAxes,
    HapticAlreadySet,
    EarlyDataRequested,
    LateDataRequested,
    InvalidOperation,
    Unsupported,
    ConfigurationFailed,
    NotPresent,
}

impl ViconSdkStatus {
    /// Returns `true` iff this status
    /// represents a success.
    pub fn is_success(&self) -> bool {
        matches!(self, ViconSdkStatus::Success)
    }
}

impl From<i32> for ViconSdkStatus {
    fn from(value: i32) -> Self {
        match value {
            0 => Self::Unknown { code: 0 },
            1 => Self::Unimplemented,
            2 => Self::Success,
            3 => Self::InvalidHostname,
            4 => Self::InvalidMulticastIp,
            5 => Self::ClientAlreadyConnected,
            6 => Self::ClientConnectionFailed,
            7 => Self::ServerAlreadyTransmittingMulticast,
            8 => Self::ServerNotTransmittingMulticast,
            9 => Self::NotConnected,
            10 => Self::NoDataFrame,
            11 => Self::InvalidIndex,
            12 => Self::InvalidCameraName,
            13 => Self::InvalidSubjectName,
            14 => Self::InvalidSegmentName,
            15 => Self::InvalidMarkerName,
            16 => Self::InvalidDeviceName,
            17 => Self::InvalidDeviceOutputName,
            18 => Self::InvalidLatencySampleRate,
            19 => Self::InvalidCoLinearAxes,
            20 => Self::LeftHandedAxes,
            21 => Self::HapticAlreadySet,
            22 => Self::EarlyDataRequested,
            23 => Self::LateDataRequested,
            24 => Self::InvalidOperation,
            25 => Self::Unsupported,
            26 => Self::ConfigurationFailed,
            27 => Self::NotPresent,
            _ => Self::Unknown { code: value },
        }
    }
}