openigtlink_rust/protocol/types/
tdata.rs

1//! TDATA (TrackingData) message type implementation
2//!
3//! The TDATA message type is used to transfer 3D positions and orientations
4//! of surgical tools, markers, etc. using transformation matrices.
5//!
6//! # Use Cases
7//!
8//! - **Surgical Tool Tracking** - Real-time tracking of multiple instruments (scalpel, probe, drill)
9//! - **Optical Tracking Systems** - NDI Polaris, Atracsys, or similar optical trackers
10//! - **Electromagnetic Tracking** - Aurora or Ascension electromagnetic trackers
11//! - **Surgical Navigation** - Displaying tool positions relative to patient anatomy
12//! - **Multi-Tool Coordination** - Tracking multiple tools simultaneously in robotic surgery
13//!
14//! # TDATA vs TRANSFORM
15//!
16//! - **TDATA**: Array of named transforms, efficient for tracking multiple tools
17//! - **TRANSFORM**: Single 4x4 matrix, simpler but requires multiple messages
18//!
19//! Use TDATA when tracking ≥2 tools to reduce network overhead.
20//!
21//! # Examples
22//!
23//! ## Tracking Multiple Surgical Instruments
24//!
25//! ```no_run
26//! use openigtlink_rust::protocol::types::{TDataMessage, TDataElement, InstrumentType};
27//! use openigtlink_rust::io::IgtlClient;
28//!
29//! let mut client = IgtlClient::connect("127.0.0.1:18944")?;
30//!
31//! let mut tdata = TDataMessage::new();
32//! tdata.set_device_name("OpticalTracker");
33//!
34//! // Tool 1: Scalpel
35//! let mut scalpel = TDataElement::new();
36//! scalpel.name = "Scalpel".to_string();
37//! scalpel.instrument_type = InstrumentType::Tool5Dof;
38//! scalpel.matrix = [
39//!     1.0, 0.0, 0.0, 100.0,  // X translation: 100mm
40//!     0.0, 1.0, 0.0, 50.0,   // Y translation: 50mm
41//!     0.0, 0.0, 1.0, 200.0,  // Z translation: 200mm
42//! ];
43//!
44//! // Tool 2: Probe
45//! let mut probe = TDataElement::new();
46//! probe.name = "Probe".to_string();
47//! probe.instrument_type = InstrumentType::Tool6Dof;
48//! probe.matrix = [
49//!     1.0, 0.0, 0.0, 150.0,
50//!     0.0, 1.0, 0.0, 75.0,
51//!     0.0, 0.0, 1.0, 180.0,
52//! ];
53//!
54//! tdata.add_element(scalpel);
55//! tdata.add_element(probe);
56//!
57//! client.send(&tdata)?;
58//! # Ok::<(), openigtlink_rust::IgtlError>(())
59//! ```
60//!
61//! ## Receiving Tracking Data at 60Hz
62//!
63//! ```no_run
64//! use openigtlink_rust::io::IgtlServer;
65//! use openigtlink_rust::protocol::types::TDataMessage;
66//! use openigtlink_rust::protocol::message::Message;
67//!
68//! let server = IgtlServer::bind("0.0.0.0:18944")?;
69//! let mut client_conn = server.accept()?;
70//!
71//! loop {
72//!     let message = client_conn.receive()?;
73//!
74//!     if message.header.message_type == "TDATA" {
75//!         let tdata = TDataMessage::from_bytes(&message.body)?;
76//!
77//!         for element in &tdata.elements {
78//!             let x = element.matrix[3];
79//!             let y = element.matrix[7];
80//!             let z = element.matrix[11];
81//!
82//!             println!("{}: position = ({:.2}, {:.2}, {:.2}) mm",
83//!                      element.name, x, y, z);
84//!         }
85//!     }
86//! }
87//! # Ok::<(), openigtlink_rust::IgtlError>(())
88//! ```
89
90use crate::protocol::message::Message;
91use crate::error::{IgtlError, Result};
92use bytes::{Buf, BufMut};
93
94/// Instrument type for tracking data
95#[derive(Debug, Clone, Copy, PartialEq, Eq)]
96#[repr(u8)]
97pub enum TrackingInstrumentType {
98    Tracker = 1,
99    Instrument6D = 2,
100    Instrument3D = 3,
101    Instrument5D = 4,
102}
103
104impl TrackingInstrumentType {
105    fn from_u8(value: u8) -> Result<Self> {
106        match value {
107            1 => Ok(TrackingInstrumentType::Tracker),
108            2 => Ok(TrackingInstrumentType::Instrument6D),
109            3 => Ok(TrackingInstrumentType::Instrument3D),
110            4 => Ok(TrackingInstrumentType::Instrument5D),
111            _ => Err(IgtlError::InvalidHeader(format!(
112                "Invalid tracking instrument type: {}",
113                value
114            ))),
115        }
116    }
117}
118
119/// Tracking data element with name, type, and transformation matrix
120#[derive(Debug, Clone, PartialEq)]
121pub struct TrackingDataElement {
122    /// Name/ID of the instrument/tracker (max 20 chars)
123    pub name: String,
124    /// Type of instrument
125    pub instrument_type: TrackingInstrumentType,
126    /// Upper 3x4 portion of 4x4 transformation matrix (row-major)
127    /// Last row [0, 0, 0, 1] is implicit
128    pub matrix: [[f32; 4]; 3],
129}
130
131impl TrackingDataElement {
132    /// Create a new tracking data element
133    pub fn new(
134        name: impl Into<String>,
135        instrument_type: TrackingInstrumentType,
136        matrix: [[f32; 4]; 3],
137    ) -> Self {
138        TrackingDataElement {
139            name: name.into(),
140            instrument_type,
141            matrix,
142        }
143    }
144
145    /// Create an identity transformation
146    pub fn identity(name: impl Into<String>, instrument_type: TrackingInstrumentType) -> Self {
147        TrackingDataElement {
148            name: name.into(),
149            instrument_type,
150            matrix: [
151                [1.0, 0.0, 0.0, 0.0],
152                [0.0, 1.0, 0.0, 0.0],
153                [0.0, 0.0, 1.0, 0.0],
154            ],
155        }
156    }
157
158    /// Create with translation only
159    pub fn with_translation(
160        name: impl Into<String>,
161        instrument_type: TrackingInstrumentType,
162        x: f32,
163        y: f32,
164        z: f32,
165    ) -> Self {
166        TrackingDataElement {
167            name: name.into(),
168            instrument_type,
169            matrix: [
170                [1.0, 0.0, 0.0, x],
171                [0.0, 1.0, 0.0, y],
172                [0.0, 0.0, 1.0, z],
173            ],
174        }
175    }
176}
177
178/// TDATA message containing multiple tracking data elements
179///
180/// # OpenIGTLink Specification
181/// - Message type: "TDATA"
182/// - Each element: NAME (char[20]) + TYPE (uint8) + Reserved (uint8) + MATRIX (float32[12])
183/// - Element size: 20 + 1 + 1 + 48 = 70 bytes
184#[derive(Debug, Clone, PartialEq)]
185pub struct TDataMessage {
186    /// List of tracking data elements
187    pub elements: Vec<TrackingDataElement>,
188}
189
190impl TDataMessage {
191    /// Create a new TDATA message with elements
192    pub fn new(elements: Vec<TrackingDataElement>) -> Self {
193        TDataMessage { elements }
194    }
195
196    /// Create an empty TDATA message
197    pub fn empty() -> Self {
198        TDataMessage {
199            elements: Vec::new(),
200        }
201    }
202
203    /// Add a tracking element
204    pub fn add_element(&mut self, element: TrackingDataElement) {
205        self.elements.push(element);
206    }
207
208    /// Get number of tracking elements
209    pub fn len(&self) -> usize {
210        self.elements.len()
211    }
212
213    /// Check if message has no elements
214    pub fn is_empty(&self) -> bool {
215        self.elements.is_empty()
216    }
217}
218
219impl Message for TDataMessage {
220    fn message_type() -> &'static str {
221        "TDATA"
222    }
223
224    fn encode_content(&self) -> Result<Vec<u8>> {
225        let mut buf = Vec::with_capacity(self.elements.len() * 70);
226
227        for element in &self.elements {
228            // Encode NAME (char[20])
229            let mut name_bytes = [0u8; 20];
230            let name_str = element.name.as_bytes();
231            let copy_len = name_str.len().min(19);
232            name_bytes[..copy_len].copy_from_slice(&name_str[..copy_len]);
233            buf.extend_from_slice(&name_bytes);
234
235            // Encode TYPE (uint8)
236            buf.put_u8(element.instrument_type as u8);
237
238            // Encode Reserved (uint8)
239            buf.put_u8(0);
240
241            // Encode MATRIX (float32[12]) - upper 3x4 portion
242            for row in &element.matrix {
243                for &val in row {
244                    buf.put_f32(val);
245                }
246            }
247        }
248
249        Ok(buf)
250    }
251
252    fn decode_content(mut data: &[u8]) -> Result<Self> {
253        let mut elements = Vec::new();
254
255        while data.len() >= 70 {
256            // Decode NAME (char[20])
257            let name_bytes = &data[..20];
258            data.advance(20);
259
260            let name_len = name_bytes.iter().position(|&b| b == 0).unwrap_or(20);
261            let name = String::from_utf8(name_bytes[..name_len].to_vec())?;
262
263            // Decode TYPE (uint8)
264            let instrument_type = TrackingInstrumentType::from_u8(data.get_u8())?;
265
266            // Decode Reserved (uint8)
267            let _reserved = data.get_u8();
268
269            // Decode MATRIX (float32[12])
270            let mut matrix = [[0.0f32; 4]; 3];
271            for row in &mut matrix {
272                for val in row {
273                    *val = data.get_f32();
274                }
275            }
276
277            elements.push(TrackingDataElement {
278                name,
279                instrument_type,
280                matrix,
281            });
282        }
283
284        if !data.is_empty() {
285            return Err(IgtlError::InvalidSize {
286                expected: 0,
287                actual: data.len(),
288            });
289        }
290
291        Ok(TDataMessage { elements })
292    }
293}
294
295#[cfg(test)]
296mod tests {
297    use super::*;
298
299    #[test]
300    fn test_message_type() {
301        assert_eq!(TDataMessage::message_type(), "TDATA");
302    }
303
304    #[test]
305    fn test_instrument_type() {
306        assert_eq!(TrackingInstrumentType::Tracker as u8, 1);
307        assert_eq!(TrackingInstrumentType::Instrument6D as u8, 2);
308        assert_eq!(TrackingInstrumentType::Instrument3D as u8, 3);
309        assert_eq!(TrackingInstrumentType::Instrument5D as u8, 4);
310    }
311
312    #[test]
313    fn test_empty() {
314        let msg = TDataMessage::empty();
315        assert!(msg.is_empty());
316        assert_eq!(msg.len(), 0);
317    }
318
319    #[test]
320    fn test_identity() {
321        let elem = TrackingDataElement::identity("Tool1", TrackingInstrumentType::Instrument6D);
322        assert_eq!(elem.matrix[0], [1.0, 0.0, 0.0, 0.0]);
323        assert_eq!(elem.matrix[1], [0.0, 1.0, 0.0, 0.0]);
324        assert_eq!(elem.matrix[2], [0.0, 0.0, 1.0, 0.0]);
325    }
326
327    #[test]
328    fn test_with_translation() {
329        let elem = TrackingDataElement::with_translation(
330            "Tool1",
331            TrackingInstrumentType::Tracker,
332            10.0,
333            20.0,
334            30.0,
335        );
336        assert_eq!(elem.matrix[0][3], 10.0);
337        assert_eq!(elem.matrix[1][3], 20.0);
338        assert_eq!(elem.matrix[2][3], 30.0);
339    }
340
341    #[test]
342    fn test_add_element() {
343        let mut msg = TDataMessage::empty();
344        msg.add_element(TrackingDataElement::identity(
345            "Tool1",
346            TrackingInstrumentType::Tracker,
347        ));
348        assert_eq!(msg.len(), 1);
349    }
350
351    #[test]
352    fn test_encode_single_element() {
353        let elem = TrackingDataElement::identity("Test", TrackingInstrumentType::Instrument6D);
354        let msg = TDataMessage::new(vec![elem]);
355        let encoded = msg.encode_content().unwrap();
356
357        assert_eq!(encoded.len(), 70);
358        // Check TYPE field
359        assert_eq!(encoded[20], 2); // Instrument6D
360        // Check Reserved field
361        assert_eq!(encoded[21], 0);
362    }
363
364    #[test]
365    fn test_roundtrip_single() {
366        let matrix = [
367            [1.0, 0.0, 0.0, 10.0],
368            [0.0, 1.0, 0.0, 20.0],
369            [0.0, 0.0, 1.0, 30.0],
370        ];
371
372        let original = TDataMessage::new(vec![TrackingDataElement::new(
373            "Tracker1",
374            TrackingInstrumentType::Tracker,
375            matrix,
376        )]);
377
378        let encoded = original.encode_content().unwrap();
379        let decoded = TDataMessage::decode_content(&encoded).unwrap();
380
381        assert_eq!(decoded.elements.len(), 1);
382        assert_eq!(decoded.elements[0].name, "Tracker1");
383        assert_eq!(
384            decoded.elements[0].instrument_type,
385            TrackingInstrumentType::Tracker
386        );
387        assert_eq!(decoded.elements[0].matrix, matrix);
388    }
389
390    #[test]
391    fn test_roundtrip_multiple() {
392        let original = TDataMessage::new(vec![
393            TrackingDataElement::identity("Tool1", TrackingInstrumentType::Instrument6D),
394            TrackingDataElement::with_translation(
395                "Tool2",
396                TrackingInstrumentType::Instrument3D,
397                5.0,
398                10.0,
399                15.0,
400            ),
401        ]);
402
403        let encoded = original.encode_content().unwrap();
404        let decoded = TDataMessage::decode_content(&encoded).unwrap();
405
406        assert_eq!(decoded.elements.len(), 2);
407        assert_eq!(decoded.elements[0].name, "Tool1");
408        assert_eq!(decoded.elements[1].name, "Tool2");
409    }
410
411    #[test]
412    fn test_name_truncation() {
413        let long_name = "ThisIsAVeryLongNameThatExceedsTwentyCharacters";
414        let elem = TrackingDataElement::identity(long_name, TrackingInstrumentType::Tracker);
415        let msg = TDataMessage::new(vec![elem]);
416
417        let encoded = msg.encode_content().unwrap();
418        let decoded = TDataMessage::decode_content(&encoded).unwrap();
419
420        assert!(decoded.elements[0].name.len() <= 19);
421    }
422
423    #[test]
424    fn test_empty_message() {
425        let msg = TDataMessage::empty();
426        let encoded = msg.encode_content().unwrap();
427        let decoded = TDataMessage::decode_content(&encoded).unwrap();
428
429        assert_eq!(decoded.elements.len(), 0);
430        assert_eq!(encoded.len(), 0);
431    }
432
433    #[test]
434    fn test_decode_invalid_size() {
435        let data = vec![0u8; 69]; // One byte short
436        let result = TDataMessage::decode_content(&data);
437        assert!(result.is_err());
438    }
439}
openigtlink_rust/protocol/types/tdata.rs

openigtlink_rust/protocol/types/
tdata.rs
