1use crate::modalities::evidential_etau::f32_to_f16;
21use bytemuck::{bytes_of, cast_slice, Pod, Zeroable};
22
23pub const SURFEL_SECTION_VERSION: u16 = 1;
24pub const SURFEL_MINI_HEADER_SIZE: usize = 64;
25pub const SURFEL_RECORD_SIZE: usize = 32;
26pub const MAGIC_SURF: [u8; 4] = *b"SURF";
27
28pub const FLAG_SURFEL_HIERARCHICAL: u16 = 1 << 0;
29pub const FLAG_SURFEL_GAUSSIAN_COVARIANCE: u16 = 1 << 1;
30
31#[repr(C)]
33#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
34pub struct SurfelSectionMiniHeader {
35 pub magic: [u8; 4],
36 pub version: u16,
37 pub flags: u16,
38 pub surfel_count: u32,
39 pub stride: u32,
40 pub sphere_center: [f32; 3],
41 pub sphere_radius: f32,
42 pub aabb_min: [f32; 3],
43 pub aabb_max: [f32; 3],
44 pub reserved: [u8; 8],
45}
46
47#[repr(C)]
49#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
50pub struct SurfelBinaryRecord {
51 pub position: [f32; 3],
52 pub normal_oct32: u32,
53 pub radii: [u16; 2],
54 pub spectral_coeffs: [u16; 3],
55 pub roughness: u16,
56 pub flags: u32,
57}
58
59#[derive(Debug, Clone, PartialEq, Eq)]
60pub enum SurfelSectionError {
61 PayloadTooShort { need: usize, have: usize },
62 BadMagic([u8; 4]),
63 UnsupportedVersion(u16),
64 BufferTooSmall { need: usize, have: usize },
65 StrideMismatch { expected: u32, actual: u32 },
66 PayloadLengthMismatch { declared: usize, actual: usize },
67 DegenerateCluster,
68}
69
70impl core::fmt::Display for SurfelSectionError {
71 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
72 match self {
73 Self::PayloadTooShort { need, have } => {
74 write!(
75 f,
76 "surfel section payload too short: have {have}, need {need}"
77 )
78 }
79 Self::BadMagic(m) => write!(f, "bad surfel section magic: {m:?}"),
80 Self::UnsupportedVersion(v) => write!(f, "unsupported surfel section version {v}"),
81 Self::BufferTooSmall { need, have } => {
82 write!(f, "buffer too small: have {have}, need {need}")
83 }
84 Self::StrideMismatch { expected, actual } => {
85 write!(
86 f,
87 "surfel stride mismatch: expected {expected}, actual {actual}"
88 )
89 }
90 Self::PayloadLengthMismatch { declared, actual } => {
91 write!(
92 f,
93 "surfel payload length mismatch: declared {declared}, actual {actual}"
94 )
95 }
96 Self::DegenerateCluster => write!(f, "degenerate cluster geometry"),
97 }
98 }
99}
100
101impl std::error::Error for SurfelSectionError {}
102
103pub fn encode_surfel_section(
105 header: &SurfelSectionMiniHeader,
106 records: &[SurfelBinaryRecord],
107 out: &mut [u8],
108) -> Result<usize, SurfelSectionError> {
109 let needed = SURFEL_MINI_HEADER_SIZE + records.len() * SURFEL_RECORD_SIZE;
110 if out.len() < needed {
111 return Err(SurfelSectionError::BufferTooSmall {
112 need: needed,
113 have: out.len(),
114 });
115 }
116
117 out[..SURFEL_MINI_HEADER_SIZE].copy_from_slice(bytes_of(header));
118 let records_bytes: &[u8] = cast_slice(records);
119 out[SURFEL_MINI_HEADER_SIZE..needed].copy_from_slice(records_bytes);
120
121 Ok(needed)
122}
123
124pub fn decode_surfel_section<'a>(
126 bytes: &'a [u8],
127) -> Result<(&'a SurfelSectionMiniHeader, &'a [SurfelBinaryRecord]), SurfelSectionError> {
128 if bytes.len() < SURFEL_MINI_HEADER_SIZE {
129 return Err(SurfelSectionError::PayloadTooShort {
130 need: SURFEL_MINI_HEADER_SIZE,
131 have: bytes.len(),
132 });
133 }
134
135 let header_bytes = &bytes[..SURFEL_MINI_HEADER_SIZE];
136 let header: &SurfelSectionMiniHeader = bytemuck::from_bytes(header_bytes);
137
138 if header.magic != MAGIC_SURF {
139 return Err(SurfelSectionError::BadMagic(header.magic));
140 }
141 if header.version != SURFEL_SECTION_VERSION {
142 return Err(SurfelSectionError::UnsupportedVersion(header.version));
143 }
144 if header.stride != SURFEL_RECORD_SIZE as u32 {
145 return Err(SurfelSectionError::StrideMismatch {
146 expected: SURFEL_RECORD_SIZE as u32,
147 actual: header.stride,
148 });
149 }
150
151 let declared_bytes = header.surfel_count as usize * SURFEL_RECORD_SIZE;
152 let actual_payload = bytes.len() - SURFEL_MINI_HEADER_SIZE;
153 if actual_payload != declared_bytes {
154 return Err(SurfelSectionError::PayloadLengthMismatch {
155 declared: declared_bytes,
156 actual: actual_payload,
157 });
158 }
159
160 let records_bytes = &bytes[SURFEL_MINI_HEADER_SIZE..];
161 let records: &[SurfelBinaryRecord] = cast_slice(records_bytes);
162
163 Ok((header, records))
164}
165
166#[inline]
171fn sign_not_zero(val: f32) -> f32 {
172 if val < 0.0 {
173 -1.0
174 } else {
175 1.0
176 }
177}
178
179#[inline]
181pub fn encode_octahedral_normal(normal: [f32; 3]) -> u32 {
182 let len = (normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]).sqrt();
183 let (mut x, mut y, z) = if len > 1e-6 {
184 (normal[0] / len, normal[1] / len, normal[2] / len)
185 } else {
186 (0.0, 0.0, 1.0)
187 };
188
189 let l1 = x.abs() + y.abs() + z.abs();
190 x /= l1;
191 y /= l1;
192
193 let mut ox = x;
194 let mut oy = y;
195 if z < 0.0 {
196 ox = (1.0 - y.abs()) * sign_not_zero(x);
197 oy = (1.0 - x.abs()) * sign_not_zero(y);
198 }
199
200 let ix = (ox.clamp(-1.0, 1.0) * i16::MAX as f32).round() as i16;
201 let iy = (oy.clamp(-1.0, 1.0) * i16::MAX as f32).round() as i16;
202
203 (ix as u16 as u32) | ((iy as u16 as u32) << 16)
204}
205
206#[inline]
208pub fn decode_octahedral_normal(packed: u32) -> [f32; 3] {
209 let ix = (packed & 0xFFFF) as i16;
210 let iy = ((packed >> 16) & 0xFFFF) as i16;
211
212 let mut x = (ix as f32 / i16::MAX as f32).max(-1.0);
213 let mut y = (iy as f32 / i16::MAX as f32).max(-1.0);
214 let z = 1.0 - x.abs() - y.abs();
215
216 if z < 0.0 {
217 let old_x = x;
218 x = (1.0 - y.abs()) * sign_not_zero(old_x);
219 y = (1.0 - old_x.abs()) * sign_not_zero(y);
220 }
221
222 let len = (x * x + y * y + z * z).sqrt();
223 if len > 1e-6 {
224 [x / len, y / len, z / len]
225 } else {
226 [0.0, 0.0, 1.0]
227 }
228}
229
230pub fn generate_surfel_from_cluster(
238 vertices: &[[f32; 3]],
239 indices: &[u32],
240 spectral_coeffs: [f32; 3],
241 roughness: f32,
242 flags: u32,
243) -> Result<SurfelBinaryRecord, SurfelSectionError> {
244 if indices.len() < 3 || indices.len() % 3 != 0 {
245 return Err(SurfelSectionError::DegenerateCluster);
246 }
247
248 let mut total_area = 0.0f32;
249 let mut weighted_centroid = [0.0f32; 3];
250 let mut weighted_normal = [0.0f32; 3];
251
252 let tri_count = indices.len() / 3;
253 for t in 0..tri_count {
254 let i0 = indices[t * 3] as usize;
255 let i1 = indices[t * 3 + 1] as usize;
256 let i2 = indices[t * 3 + 2] as usize;
257
258 if i0 >= vertices.len() || i1 >= vertices.len() || i2 >= vertices.len() {
259 return Err(SurfelSectionError::DegenerateCluster);
260 }
261
262 let v0 = vertices[i0];
263 let v1 = vertices[i1];
264 let v2 = vertices[i2];
265
266 let e1 = [v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]];
267 let e2 = [v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]];
268
269 let cross = [
270 e1[1] * e2[2] - e1[2] * e2[1],
271 e1[2] * e2[0] - e1[0] * e2[2],
272 e1[0] * e2[1] - e1[1] * e2[0],
273 ];
274
275 let area2 = (cross[0] * cross[0] + cross[1] * cross[1] + cross[2] * cross[2]).sqrt();
276 let area = area2 * 0.5;
277
278 if area > 1e-8 {
279 total_area += area;
280 let tri_centroid = [
281 (v0[0] + v1[0] + v2[0]) / 3.0,
282 (v0[1] + v1[1] + v2[1]) / 3.0,
283 (v0[2] + v1[2] + v2[2]) / 3.0,
284 ];
285 weighted_centroid[0] += tri_centroid[0] * area;
286 weighted_centroid[1] += tri_centroid[1] * area;
287 weighted_centroid[2] += tri_centroid[2] * area;
288
289 weighted_normal[0] += cross[0];
290 weighted_normal[1] += cross[1];
291 weighted_normal[2] += cross[2];
292 }
293 }
294
295 if total_area <= 1e-8 {
296 return Err(SurfelSectionError::DegenerateCluster);
297 }
298
299 let centroid = [
300 weighted_centroid[0] / total_area,
301 weighted_centroid[1] / total_area,
302 weighted_centroid[2] / total_area,
303 ];
304
305 let norm_len = (weighted_normal[0] * weighted_normal[0]
306 + weighted_normal[1] * weighted_normal[1]
307 + weighted_normal[2] * weighted_normal[2])
308 .sqrt();
309
310 let unit_normal = if norm_len > 1e-6 {
311 [
312 weighted_normal[0] / norm_len,
313 weighted_normal[1] / norm_len,
314 weighted_normal[2] / norm_len,
315 ]
316 } else {
317 [0.0, 0.0, 1.0]
318 };
319
320 let normal_oct = encode_octahedral_normal(unit_normal);
321
322 let mut var_dist_sq = 0.0f32;
324 let mut max_dist_sq = 0.0f32;
325 for &idx in indices {
326 let v = vertices[idx as usize];
327 let d = [v[0] - centroid[0], v[1] - centroid[1], v[2] - centroid[2]];
328 let d_sq = d[0] * d[0] + d[1] * d[1] + d[2] * d[2];
329 var_dist_sq += d_sq;
330 if d_sq > max_dist_sq {
331 max_dist_sq = d_sq;
332 }
333 }
334
335 let avg_radius = (var_dist_sq / (indices.len() as f32)).sqrt().max(0.01);
336 let max_radius = max_dist_sq.sqrt().max(avg_radius);
337
338 let r_major = f32_to_f16(max_radius);
340 let r_minor = f32_to_f16(avg_radius);
341
342 let c0 = f32_to_f16(spectral_coeffs[0]);
343 let c1 = f32_to_f16(spectral_coeffs[1]);
344 let c2 = f32_to_f16(spectral_coeffs[2]);
345 let rough = f32_to_f16(roughness.clamp(0.0, 1.0));
346
347 Ok(SurfelBinaryRecord {
348 position: centroid,
349 normal_oct32: normal_oct,
350 radii: [r_major, r_minor],
351 spectral_coeffs: [c0, c1, c2],
352 roughness: rough,
353 flags,
354 })
355}
356
357#[cfg(test)]
358mod tests {
359 use super::*;
360
361 #[test]
362 fn test_sizes_and_alignment() {
363 assert_eq!(std::mem::size_of::<SurfelSectionMiniHeader>(), 64);
364 assert_eq!(std::mem::size_of::<SurfelBinaryRecord>(), 32);
365 }
366
367 #[test]
368 fn test_octahedral_normal_roundtrip() {
369 for n in [
370 [0.0, 0.0, 1.0],
371 [0.0, 0.0, -1.0],
372 [1.0, 0.0, 0.0],
373 [0.0, 1.0, 0.0],
374 [0.57735, 0.57735, 0.57735],
375 [-0.57735, 0.57735, -0.57735],
376 ] {
377 let packed = encode_octahedral_normal(n);
378 let recovered = decode_octahedral_normal(packed);
379 let dot = n[0] * recovered[0] + n[1] * recovered[1] + n[2] * recovered[2];
380 assert!(dot > 0.999, "normal roundtrip dot error: {dot}");
381 }
382 }
383
384 #[test]
385 fn test_surfel_section_encode_decode() {
386 let header = SurfelSectionMiniHeader {
387 magic: MAGIC_SURF,
388 version: SURFEL_SECTION_VERSION,
389 flags: FLAG_SURFEL_HIERARCHICAL,
390 surfel_count: 2,
391 stride: 32,
392 sphere_center: [0.0, 1.0, 0.0],
393 sphere_radius: 5.0,
394 aabb_min: [-5.0, -4.0, -5.0],
395 aabb_max: [5.0, 6.0, 5.0],
396 reserved: [0; 8],
397 };
398
399 let records = [
400 SurfelBinaryRecord {
401 position: [1.0, 2.0, 3.0],
402 normal_oct32: encode_octahedral_normal([0.0, 1.0, 0.0]),
403 radii: [f32_to_f16(0.5), f32_to_f16(0.3)],
404 spectral_coeffs: [f32_to_f16(0.1), f32_to_f16(-0.2), f32_to_f16(0.4)],
405 roughness: f32_to_f16(0.8),
406 flags: 0,
407 },
408 SurfelBinaryRecord {
409 position: [-1.0, 0.5, 2.0],
410 normal_oct32: encode_octahedral_normal([1.0, 0.0, 0.0]),
411 radii: [f32_to_f16(0.6), f32_to_f16(0.4)],
412 spectral_coeffs: [f32_to_f16(0.0), f32_to_f16(0.0), f32_to_f16(0.5)],
413 roughness: f32_to_f16(0.2),
414 flags: 1,
415 },
416 ];
417
418 let mut buf = [0u8; 128];
419 let bytes_written = encode_surfel_section(&header, &records, &mut buf).unwrap();
420 assert_eq!(bytes_written, 64 + 64);
421
422 let (dec_header, dec_records) = decode_surfel_section(&buf[..bytes_written]).unwrap();
423 assert_eq!(dec_header.surfel_count, 2);
424 assert_eq!(dec_records.len(), 2);
425 assert_eq!(dec_records[0].position, [1.0, 2.0, 3.0]);
426 }
427
428 #[test]
429 fn test_generate_surfel_from_cluster() {
430 let vertices = [
431 [0.0, 0.0, 0.0],
432 [1.0, 0.0, 0.0],
433 [0.0, 1.0, 0.0],
434 [1.0, 1.0, 0.0],
435 ];
436 let indices = [0, 1, 2, 1, 3, 2]; let surfel =
439 generate_surfel_from_cluster(&vertices, &indices, [0.1, -0.5, 0.8], 0.4, 0).unwrap();
440
441 assert!((surfel.position[0] - 0.5).abs() < 1e-4);
442 assert!((surfel.position[1] - 0.5).abs() < 1e-4);
443 assert!((surfel.position[2] - 0.0).abs() < 1e-4);
444
445 let normal = decode_octahedral_normal(surfel.normal_oct32);
446 assert!(normal[2] > 0.99); }
448}