lnmp_spatial/
encoder.rs

1use crate::error::SpatialError;
2use crate::types::*;
3use bytes::BufMut;
4
5pub fn encode_spatial(value: &SpatialValue, buf: &mut Vec<u8>) -> Result<(), SpatialError> {
6    // Flag for Spatial Mode (0x07) - This might be handled at a higher level,
7    // but per spec: [SPATIAL_FLAG][TYPE_ID][FLOAT32]...
8    // Assuming SPATIAL_FLAG is the mode byte 0x07, which is likely written by the container.
9    // Here we write [TYPE_ID][DATA]
10
11    match value {
12        SpatialValue::S1(v) => {
13            buf.put_u8(SpatialType::Position2D as u8);
14            buf.put_f32(v.x);
15            buf.put_f32(v.y);
16        }
17        SpatialValue::S2(v) => {
18            buf.put_u8(SpatialType::Position3D as u8);
19            buf.put_f32(v.x);
20            buf.put_f32(v.y);
21            buf.put_f32(v.z);
22        }
23        SpatialValue::S3(v) => {
24            buf.put_u8(SpatialType::Rotation as u8);
25            buf.put_f32(v.pitch);
26            buf.put_f32(v.yaw);
27            buf.put_f32(v.roll);
28        }
29        SpatialValue::S4(v) => {
30            buf.put_u8(SpatialType::Velocity as u8);
31            buf.put_f32(v.vx);
32            buf.put_f32(v.vy);
33            buf.put_f32(v.vz);
34        }
35        SpatialValue::S5(v) => {
36            buf.put_u8(SpatialType::Acceleration as u8);
37            buf.put_f32(v.ax);
38            buf.put_f32(v.ay);
39            buf.put_f32(v.az);
40        }
41        SpatialValue::S6(v) => {
42            buf.put_u8(SpatialType::BoundingBox as u8);
43            buf.put_f32(v.min_x);
44            buf.put_f32(v.min_y);
45            buf.put_f32(v.min_z);
46            buf.put_f32(v.max_x);
47            buf.put_f32(v.max_y);
48            buf.put_f32(v.max_z);
49        }
50        SpatialValue::S7(v) => {
51            buf.put_u8(SpatialType::Quaternion as u8);
52            buf.put_f32(v.qx);
53            buf.put_f32(v.qy);
54            buf.put_f32(v.qz);
55            buf.put_f32(v.qw);
56        }
57        SpatialValue::S8(v) => {
58            buf.put_u8(SpatialType::Path as u8);
59            buf.put_u32(v.points.len() as u32);
60            for point in &v.points {
61                buf.put_f32(point.x);
62                buf.put_f32(point.y);
63                buf.put_f32(point.z);
64            }
65        }
66        SpatialValue::S9(v) => {
67            buf.put_u8(SpatialType::Transform as u8);
68            buf.put_f32(v.position.x);
69            buf.put_f32(v.position.y);
70            buf.put_f32(v.position.z);
71            buf.put_f32(v.rotation.pitch);
72            buf.put_f32(v.rotation.yaw);
73            buf.put_f32(v.rotation.roll);
74            buf.put_f32(v.scale.x);
75            buf.put_f32(v.scale.y);
76            buf.put_f32(v.scale.z);
77        }
78        SpatialValue::S10(v) => {
79            buf.put_u8(SpatialType::SpatialState as u8);
80            // Bitmask for presence: [Pos][Rot][Vel][Acc]
81            let mut mask: u8 = 0;
82            if v.position.is_some() {
83                mask |= 0x01;
84            }
85            if v.rotation.is_some() {
86                mask |= 0x02;
87            }
88            if v.velocity.is_some() {
89                mask |= 0x04;
90            }
91            if v.acceleration.is_some() {
92                mask |= 0x08;
93            }
94            buf.put_u8(mask);
95
96            if let Some(p) = v.position {
97                buf.put_f32(p.x);
98                buf.put_f32(p.y);
99                buf.put_f32(p.z);
100            }
101            if let Some(r) = v.rotation {
102                buf.put_f32(r.pitch);
103                buf.put_f32(r.yaw);
104                buf.put_f32(r.roll);
105            }
106            if let Some(vel) = v.velocity {
107                buf.put_f32(vel.vx);
108                buf.put_f32(vel.vy);
109                buf.put_f32(vel.vz);
110            }
111            if let Some(acc) = v.acceleration {
112                buf.put_f32(acc.ax);
113                buf.put_f32(acc.ay);
114                buf.put_f32(acc.az);
115            }
116        }
117        SpatialValue::S11(v) => {
118            buf.put_u8(SpatialType::PositionDelta as u8);
119            buf.put_f32(v.dx);
120            buf.put_f32(v.dy);
121            buf.put_f32(v.dz);
122        }
123        SpatialValue::S12(v) => {
124            buf.put_u8(SpatialType::RotationDelta as u8);
125            buf.put_f32(v.d_pitch);
126            buf.put_f32(v.d_yaw);
127            buf.put_f32(v.d_roll);
128        }
129        SpatialValue::S13(v) => {
130            buf.put_u8(SpatialType::SpatialDelta as u8);
131            match v {
132                SpatialDelta::Position(p) => {
133                    buf.put_u8(0x01); // Type: Position
134                    buf.put_f32(p.dx);
135                    buf.put_f32(p.dy);
136                    buf.put_f32(p.dz);
137                }
138                SpatialDelta::Rotation(r) => {
139                    buf.put_u8(0x02); // Type: Rotation
140                    buf.put_f32(r.d_pitch);
141                    buf.put_f32(r.d_yaw);
142                    buf.put_f32(r.d_roll);
143                }
144                SpatialDelta::State {
145                    position,
146                    rotation,
147                    velocity,
148                    acceleration,
149                } => {
150                    buf.put_u8(0x03); // Type: State
151                                      // Bitmask: [Pos][Rot][Vel][Acc]
152                    let mut mask: u8 = 0;
153                    if position.is_some() {
154                        mask |= 0x01;
155                    }
156                    if rotation.is_some() {
157                        mask |= 0x02;
158                    }
159                    if velocity.is_some() {
160                        mask |= 0x04;
161                    }
162                    if acceleration.is_some() {
163                        mask |= 0x08;
164                    }
165                    buf.put_u8(mask);
166
167                    if let Some(p) = position {
168                        buf.put_f32(p.dx);
169                        buf.put_f32(p.dy);
170                        buf.put_f32(p.dz);
171                    }
172                    if let Some(r) = rotation {
173                        buf.put_f32(r.d_pitch);
174                        buf.put_f32(r.d_yaw);
175                        buf.put_f32(r.d_roll);
176                    }
177                    if let Some(vel) = velocity {
178                        buf.put_f32(vel.vx);
179                        buf.put_f32(vel.vy);
180                        buf.put_f32(vel.vz);
181                    }
182                    if let Some(acc) = acceleration {
183                        buf.put_f32(acc.ax);
184                        buf.put_f32(acc.ay);
185                        buf.put_f32(acc.az);
186                    }
187                }
188            }
189        }
190    }
191    Ok(())
192}
lnmp_spatial/encoder.rs

lnmp_spatial/
encoder.rs
