1use bytemuck::{bytes_of, pod_read_unaligned, Pod, Zeroable};
9
10use super::cluster_dag_section::{decode_oct16_normal, encode_oct16_normal};
11
12pub const MAGIC_FEMC: [u8; 4] = *b"FEMC";
14
15pub const CORRESPONDENCE_SECTION_VERSION: u16 = 1;
17
18pub const CORRESPONDENCE_HEADER_SIZE: usize = 16;
20
21pub const VERTEX_TET_BINDING_SIZE: usize = 16;
23
24pub const BARYCENTRIC_UNORM16_SUM: u32 = 65_535;
26
27pub const MAX_CORRESPONDENCE_BINDINGS: usize = 1_048_576;
29
30pub const FLAG_FEMC_CONFORMING_BARYCENTRIC: u16 = 1 << 0;
32
33pub const FLAG_FEMC_HAS_NORMAL_ROTATION: u16 = 1 << 1;
35
36#[repr(C)]
38#[derive(Debug, Clone, Copy, PartialEq, Eq, Pod, Zeroable)]
39pub struct CorrespondenceMiniHeader {
40 pub magic: [u8; 4],
41 pub version: u16,
42 pub flags: u16,
43 pub binding_count: u32,
44 pub tet_cell_count: u32,
45}
46
47const _: () =
48 assert!(core::mem::size_of::<CorrespondenceMiniHeader>() == CORRESPONDENCE_HEADER_SIZE);
49
50impl Default for CorrespondenceMiniHeader {
51 fn default() -> Self {
52 Self {
53 magic: MAGIC_FEMC,
54 version: CORRESPONDENCE_SECTION_VERSION,
55 flags: FLAG_FEMC_CONFORMING_BARYCENTRIC | FLAG_FEMC_HAS_NORMAL_ROTATION,
56 binding_count: 0,
57 tet_cell_count: 0,
58 }
59 }
60}
61
62#[repr(C)]
64#[derive(Debug, Clone, Copy, PartialEq, Eq, Pod, Zeroable)]
65pub struct VertexTetBinding {
66 pub tet_element_index: u32,
68 pub barycentric_unorm16: [u16; 4],
70 pub normal_rotation_oct16: [i16; 2],
72}
73
74const _: () = assert!(core::mem::size_of::<VertexTetBinding>() == VERTEX_TET_BINDING_SIZE);
75
76impl VertexTetBinding {
77 #[inline]
80 pub fn from_barycentric(
81 tet_element_index: u32,
82 weights: [f32; 4],
83 normal_dir: [f32; 3],
84 ) -> Self {
85 let mut clamped = [0.0f32; 4];
86 for i in 0..4 {
87 let w = weights[i];
88 clamped[i] = if w.is_finite() && w > 0.0 { w } else { 0.0 };
89 }
90 let sum = clamped[0] + clamped[1] + clamped[2] + clamped[3];
91 let unorm = if sum <= 1e-12 {
92 [65_535u16, 0, 0, 0]
93 } else {
94 let mut q = [0u16; 4];
95 let mut max_idx = 0usize;
96 let mut q_sum = 0i32;
97 for i in 0..4 {
98 if clamped[i] > clamped[max_idx] {
99 max_idx = i;
100 }
101 let scaled = ((clamped[i] / sum) * 65_535.0).round().clamp(0.0, 65_535.0) as u16;
102 q[i] = scaled;
103 q_sum += scaled as i32;
104 }
105 let diff = 65_535i32 - q_sum;
106 q[max_idx] = (q[max_idx] as i32 + diff).clamp(0, 65_535) as u16;
107 q
108 };
109
110 Self {
111 tet_element_index,
112 barycentric_unorm16: unorm,
113 normal_rotation_oct16: encode_oct16_normal(normal_dir),
114 }
115 }
116
117 #[inline]
119 pub fn barycentric_weights(&self) -> [f32; 4] {
120 const INV: f32 = 1.0 / 65_535.0;
121 [
122 self.barycentric_unorm16[0] as f32 * INV,
123 self.barycentric_unorm16[1] as f32 * INV,
124 self.barycentric_unorm16[2] as f32 * INV,
125 self.barycentric_unorm16[3] as f32 * INV,
126 ]
127 }
128
129 #[inline]
131 pub fn interpolate_deformed_vertex(&self, tet_corners: [[f32; 3]; 4]) -> [f32; 3] {
132 let w = self.barycentric_weights();
133 [
134 w[0] * tet_corners[0][0]
135 + w[1] * tet_corners[1][0]
136 + w[2] * tet_corners[2][0]
137 + w[3] * tet_corners[3][0],
138 w[0] * tet_corners[0][1]
139 + w[1] * tet_corners[1][1]
140 + w[2] * tet_corners[2][1]
141 + w[3] * tet_corners[3][1],
142 w[0] * tet_corners[0][2]
143 + w[1] * tet_corners[1][2]
144 + w[2] * tet_corners[2][2]
145 + w[3] * tet_corners[3][2],
146 ]
147 }
148
149 #[inline]
151 pub fn decoded_normal_dir(&self) -> [f32; 3] {
152 decode_oct16_normal(self.normal_rotation_oct16)
153 }
154}
155
156#[derive(Debug, Clone, PartialEq, Eq)]
158pub enum CorrespondenceSectionError {
159 PayloadTooShort {
160 need: usize,
161 have: usize,
162 },
163 BadMagic([u8; 4]),
164 UnsupportedVersion(u16),
165 TooManyBindings {
166 count: usize,
167 max: usize,
168 },
169 PayloadLengthMismatch {
170 declared_bindings: u32,
171 payload_bytes: usize,
172 },
173 BufferTooSmall {
174 need: usize,
175 have: usize,
176 },
177 TetIndexOutOfBounds {
178 binding_index: usize,
179 tet_index: u32,
180 tet_cell_count: u32,
181 },
182 InvalidBarycentricSum {
183 binding_index: usize,
184 sum: u32,
185 },
186 TetVertexIndexOutOfBounds {
187 tet_index: usize,
188 vertex_index: u32,
189 vertex_count: usize,
190 },
191}
192
193impl core::fmt::Display for CorrespondenceSectionError {
194 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
195 match self {
196 Self::PayloadTooShort { need, have } => {
197 write!(f, "FEMC payload too short: have {have}, need {need}")
198 }
199 Self::BadMagic(m) => write!(f, "FEMC bad magic: {m:?}"),
200 Self::UnsupportedVersion(v) => write!(f, "FEMC unsupported version: {v}"),
201 Self::TooManyBindings { count, max } => {
202 write!(f, "FEMC binding count {count} exceeds max {max}")
203 }
204 Self::PayloadLengthMismatch {
205 declared_bindings,
206 payload_bytes,
207 } => write!(
208 f,
209 "FEMC payload length {payload_bytes} does not match {declared_bindings} bindings"
210 ),
211 Self::BufferTooSmall { need, have } => {
212 write!(f, "FEMC buffer too small: have {have}, need {need}")
213 }
214 Self::TetIndexOutOfBounds {
215 binding_index,
216 tet_index,
217 tet_cell_count,
218 } => write!(
219 f,
220 "FEMC binding[{binding_index}] tet_index {tet_index} >= tet_cell_count {tet_cell_count}"
221 ),
222 Self::InvalidBarycentricSum { binding_index, sum } => write!(
223 f,
224 "FEMC binding[{binding_index}] barycentric sum {sum} != {BARYCENTRIC_UNORM16_SUM}"
225 ),
226 Self::TetVertexIndexOutOfBounds {
227 tet_index,
228 vertex_index,
229 vertex_count,
230 } => write!(
231 f,
232 "FEMC tet[{tet_index}] vertex {vertex_index} >= vertex_count {vertex_count}"
233 ),
234 }
235 }
236}
237
238impl std::error::Error for CorrespondenceSectionError {}
239
240#[inline]
242pub const fn correspondence_encoded_len(binding_count: usize) -> Option<usize> {
243 if binding_count > MAX_CORRESPONDENCE_BINDINGS {
244 return None;
245 }
246 match binding_count.checked_mul(VERTEX_TET_BINDING_SIZE) {
247 Some(body) => CORRESPONDENCE_HEADER_SIZE.checked_add(body),
248 None => None,
249 }
250}
251
252fn validate_binding(
253 index: usize,
254 binding: &VertexTetBinding,
255 tet_cell_count: u32,
256) -> Result<(), CorrespondenceSectionError> {
257 if tet_cell_count > 0 && binding.tet_element_index >= tet_cell_count {
258 return Err(CorrespondenceSectionError::TetIndexOutOfBounds {
259 binding_index: index,
260 tet_index: binding.tet_element_index,
261 tet_cell_count,
262 });
263 }
264 let sum = binding.barycentric_unorm16[0] as u32
265 + binding.barycentric_unorm16[1] as u32
266 + binding.barycentric_unorm16[2] as u32
267 + binding.barycentric_unorm16[3] as u32;
268 if sum != BARYCENTRIC_UNORM16_SUM {
269 return Err(CorrespondenceSectionError::InvalidBarycentricSum {
270 binding_index: index,
271 sum,
272 });
273 }
274 Ok(())
275}
276
277pub fn encode_correspondence_section(
279 header: &CorrespondenceMiniHeader,
280 bindings: &[VertexTetBinding],
281 out: &mut [u8],
282) -> Result<usize, CorrespondenceSectionError> {
283 if header.magic != MAGIC_FEMC {
284 return Err(CorrespondenceSectionError::BadMagic(header.magic));
285 }
286 if header.version != CORRESPONDENCE_SECTION_VERSION {
287 return Err(CorrespondenceSectionError::UnsupportedVersion(
288 header.version,
289 ));
290 }
291 let needed = correspondence_encoded_len(bindings.len()).ok_or(
292 CorrespondenceSectionError::TooManyBindings {
293 count: bindings.len(),
294 max: MAX_CORRESPONDENCE_BINDINGS,
295 },
296 )?;
297 if out.len() < needed {
298 return Err(CorrespondenceSectionError::BufferTooSmall {
299 need: needed,
300 have: out.len(),
301 });
302 }
303 for (idx, b) in bindings.iter().enumerate() {
304 validate_binding(idx, b, header.tet_cell_count)?;
305 }
306
307 let mut canonical_header = *header;
308 canonical_header.binding_count = bindings.len() as u32;
309 out[..CORRESPONDENCE_HEADER_SIZE].copy_from_slice(bytes_of(&canonical_header));
310
311 let mut offset = CORRESPONDENCE_HEADER_SIZE;
312 for b in bindings {
313 out[offset..offset + VERTEX_TET_BINDING_SIZE].copy_from_slice(bytes_of(b));
314 offset += VERTEX_TET_BINDING_SIZE;
315 }
316 Ok(needed)
317}
318
319pub fn decode_correspondence_section(
321 payload: &[u8],
322 out_bindings: &mut [VertexTetBinding],
323) -> Result<(CorrespondenceMiniHeader, usize), CorrespondenceSectionError> {
324 if payload.len() < CORRESPONDENCE_HEADER_SIZE {
325 return Err(CorrespondenceSectionError::PayloadTooShort {
326 need: CORRESPONDENCE_HEADER_SIZE,
327 have: payload.len(),
328 });
329 }
330 let header: CorrespondenceMiniHeader = pod_read_unaligned(&payload[..CORRESPONDENCE_HEADER_SIZE]);
331 if header.magic != MAGIC_FEMC {
332 return Err(CorrespondenceSectionError::BadMagic(header.magic));
333 }
334 if header.version != CORRESPONDENCE_SECTION_VERSION {
335 return Err(CorrespondenceSectionError::UnsupportedVersion(
336 header.version,
337 ));
338 }
339 let count = header.binding_count as usize;
340 let needed_bytes = correspondence_encoded_len(count).ok_or(
341 CorrespondenceSectionError::TooManyBindings {
342 count,
343 max: MAX_CORRESPONDENCE_BINDINGS,
344 },
345 )?;
346 if payload.len() != needed_bytes {
347 return Err(CorrespondenceSectionError::PayloadLengthMismatch {
348 declared_bindings: header.binding_count,
349 payload_bytes: payload.len(),
350 });
351 }
352 if out_bindings.len() < count {
353 return Err(CorrespondenceSectionError::BufferTooSmall {
354 need: count,
355 have: out_bindings.len(),
356 });
357 }
358
359 let mut offset = CORRESPONDENCE_HEADER_SIZE;
360 for (idx, slot) in out_bindings[..count].iter_mut().enumerate() {
361 let binding: VertexTetBinding =
362 pod_read_unaligned(&payload[offset..offset + VERTEX_TET_BINDING_SIZE]);
363 validate_binding(idx, &binding, header.tet_cell_count)?;
364 *slot = binding;
365 offset += VERTEX_TET_BINDING_SIZE;
366 }
367 Ok((header, count))
368}
369
370pub fn deform_surface_vertices_zero_heap(
372 bindings: &[VertexTetBinding],
373 tet_elements: &[[u32; 4]],
374 tet_vertices: &[[f32; 3]],
375 out_surface_vertices: &mut [[f32; 3]],
376) -> Result<usize, CorrespondenceSectionError> {
377 if out_surface_vertices.len() < bindings.len() {
378 return Err(CorrespondenceSectionError::BufferTooSmall {
379 need: bindings.len(),
380 have: out_surface_vertices.len(),
381 });
382 }
383 for (idx, binding) in bindings.iter().enumerate() {
384 let tet_idx = binding.tet_element_index as usize;
385 if tet_idx >= tet_elements.len() {
386 return Err(CorrespondenceSectionError::TetIndexOutOfBounds {
387 binding_index: idx,
388 tet_index: binding.tet_element_index,
389 tet_cell_count: tet_elements.len() as u32,
390 });
391 }
392 let tet = tet_elements[tet_idx];
393 let mut corners = [[0.0f32; 3]; 4];
394 for k in 0..4 {
395 let vi = tet[k] as usize;
396 if vi >= tet_vertices.len() {
397 return Err(CorrespondenceSectionError::TetVertexIndexOutOfBounds {
398 tet_index: tet_idx,
399 vertex_index: tet[k],
400 vertex_count: tet_vertices.len(),
401 });
402 }
403 corners[k] = tet_vertices[vi];
404 }
405 out_surface_vertices[idx] = binding.interpolate_deformed_vertex(corners);
406 }
407 Ok(bindings.len())
408}
409
410#[cfg(test)]
411mod tests {
412 use super::*;
413
414 #[test]
415 fn femc_layout_and_barycentric_partition_of_unity() {
416 assert_eq!(core::mem::size_of::<CorrespondenceMiniHeader>(), 16);
417 assert_eq!(core::mem::size_of::<VertexTetBinding>(), 16);
418
419 let b = VertexTetBinding::from_barycentric(
420 0,
421 [0.25, 0.25, 0.25, 0.25],
422 [0.0, 1.0, 0.0],
423 );
424 let sum: u32 = b.barycentric_unorm16.iter().map(|&w| w as u32).sum();
425 assert_eq!(sum, BARYCENTRIC_UNORM16_SUM);
426
427 let corners = [
428 [0.0, 0.0, 0.0],
429 [2.0, 0.0, 0.0],
430 [0.0, 2.0, 0.0],
431 [0.0, 0.0, 2.0],
432 ];
433 let pos = b.interpolate_deformed_vertex(corners);
434 assert!((pos[0] - 0.5).abs() < 1e-4);
435 assert!((pos[1] - 0.5).abs() < 1e-4);
436 assert!((pos[2] - 0.5).abs() < 1e-4);
437 }
438
439 #[test]
440 fn femc_encode_decode_and_deform_roundtrip() {
441 let bindings = [
442 VertexTetBinding::from_barycentric(0, [1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
443 VertexTetBinding::from_barycentric(0, [0.1, 0.2, 0.3, 0.4], [1.0, 0.0, 0.0]),
444 ];
445 let header = CorrespondenceMiniHeader {
446 binding_count: 2,
447 tet_cell_count: 1,
448 ..CorrespondenceMiniHeader::default()
449 };
450 let mut buf = [0u8; 16 + 2 * 16];
451 let n = encode_correspondence_section(&header, &bindings, &mut buf).unwrap();
452 assert_eq!(n, buf.len());
453
454 let mut decoded = [VertexTetBinding::zeroed(); 2];
455 let (dec_hdr, count) = decode_correspondence_section(&buf, &mut decoded).unwrap();
456 assert_eq!(count, 2);
457 assert_eq!(dec_hdr, header);
458 assert_eq!(decoded, bindings);
459
460 let tets = [[0u32, 1, 2, 3]];
461 let verts = [
462 [1.0, 2.0, 3.0],
463 [2.0, 2.0, 3.0],
464 [1.0, 4.0, 3.0],
465 [1.0, 2.0, 7.0],
466 ];
467 let mut out_surface = [[0.0f32; 3]; 2];
468 let count_def =
469 deform_surface_vertices_zero_heap(&decoded, &tets, &verts, &mut out_surface).unwrap();
470 assert_eq!(count_def, 2);
471 assert!((out_surface[0][0] - 1.0).abs() < 1e-5);
472 assert!((out_surface[0][1] - 2.0).abs() < 1e-5);
473 assert!((out_surface[0][2] - 3.0).abs() < 1e-5);
474 }
475}