1use bytemuck::{Pod, Zeroable};
10use qualia_cooperative_core::contextual_evidence::{
11 EpistemicTier, ObservationMethod, ObserverRole, TemporalStabilityClass,
12};
13
14use super::SkeletalSectionError;
15use crate::container_10d::crc32c::crc32c;
16
17pub const SKL2_MAGIC: [u8; 4] = *b"SKL2";
19pub const SSM1_MAGIC: [u8; 4] = *b"SSM1";
21pub const SKL2_VERSION_1: u16 = 1;
23pub const SKL2_VERSION_2: u16 = 2;
25pub const SKL2_HEADER_LEN: usize = 32;
27pub const SKL2_NONE_INDEX: u16 = u16::MAX;
29pub const SKL2_MAX_JOINTS: usize = 256;
31pub const SKL2_MAX_CONTAINER_BYTES: usize = 42 * 1024 * 1024;
33
34pub const SKL2_JOINT_FLAG_SYMMETRIC: u8 = 1 << 0;
36pub const SKL2_JOINT_FLAG_VESTIGIAL: u8 = 1 << 1;
37pub const SKL2_JOINT_FLAG_HYDROSTATIC: u8 = 1 << 2;
38pub const SKL2_JOINT_FLAG_EXOSKELETAL: u8 = 1 << 3;
39pub const SKL2_JOINT_FLAG_LOOP_ANCHOR: u8 = 1 << 4;
40
41#[repr(C)]
43#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable, serde::Serialize, serde::Deserialize)]
44pub struct SwingTwistConeLimits {
45 pub swing_y_rad: f32,
46 pub swing_z_rad: f32,
47 pub twist_min_rad: f32,
48 pub twist_max_rad: f32,
49}
50
51impl SwingTwistConeLimits {
52 #[inline]
53 pub const fn new(
54 swing_y_rad: f32,
55 swing_z_rad: f32,
56 twist_min_rad: f32,
57 twist_max_rad: f32,
58 ) -> Self {
59 Self {
60 swing_y_rad,
61 swing_z_rad,
62 twist_min_rad,
63 twist_max_rad,
64 }
65 }
66
67 #[inline]
68 pub const fn symmetric(swing_rad: f32, twist_half_rad: f32) -> Self {
69 Self {
70 swing_y_rad: swing_rad,
71 swing_z_rad: swing_rad,
72 twist_min_rad: -twist_half_rad,
73 twist_max_rad: twist_half_rad,
74 }
75 }
76
77 #[inline]
78 pub const fn rigid() -> Self {
79 Self::new(0.0, 0.0, 0.0, 0.0)
80 }
81
82 #[inline]
83 pub fn validate(&self) -> Result<(), SkeletalSectionError> {
84 if !self.swing_y_rad.is_finite()
85 || !self.swing_z_rad.is_finite()
86 || !self.twist_min_rad.is_finite()
87 || !self.twist_max_rad.is_finite()
88 {
89 return Err(SkeletalSectionError::InvalidData);
90 }
91 if self.swing_y_rad < 0.0
92 || self.swing_z_rad < 0.0
93 || self.swing_y_rad > core::f32::consts::PI
94 || self.swing_z_rad > core::f32::consts::PI
95 || self.twist_min_rad > self.twist_max_rad
96 || self.twist_min_rad < -core::f32::consts::TAU
97 || self.twist_max_rad > core::f32::consts::TAU
98 {
99 return Err(SkeletalSectionError::InvalidData);
100 }
101 Ok(())
102 }
103}
104
105impl Default for SwingTwistConeLimits {
106 fn default() -> Self {
107 Self::symmetric(core::f32::consts::FRAC_PI_3, core::f32::consts::FRAC_PI_4)
108 }
109}
110
111#[repr(u8)]
113#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default, serde::Serialize, serde::Deserialize)]
114pub enum KinematicSolverRegime {
115 #[default]
116 ArticulatedRigidBodyDca = 0,
117 CosseratRodContinuum = 1,
118 ExoskeletalCuticleShell = 2,
119 HydrostaticMuscularContinuum = 3,
120 TurgorAuxinGrowth = 4,
121}
122
123impl KinematicSolverRegime {
124 #[inline]
125 pub const fn as_u8(self) -> u8 {
126 self as u8
127 }
128
129 #[inline]
130 pub const fn from_u8(raw: u8) -> Result<Self, SkeletalSectionError> {
131 match raw {
132 0 => Ok(Self::ArticulatedRigidBodyDca),
133 1 => Ok(Self::CosseratRodContinuum),
134 2 => Ok(Self::ExoskeletalCuticleShell),
135 3 => Ok(Self::HydrostaticMuscularContinuum),
136 4 => Ok(Self::TurgorAuxinGrowth),
137 _ => Err(SkeletalSectionError::InvalidData),
138 }
139 }
140}
141
142#[repr(transparent)]
145#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default, Pod, Zeroable)]
146pub struct EpistemicProvenanceWord(pub u32);
147
148impl EpistemicProvenanceWord {
149 #[inline]
150 pub const fn pack(
151 status: u8,
152 method_class: u8,
153 observer_role: u8,
154 stability_class: u8,
155 freshness_q10: u16,
156 confidence_q8: u8,
157 ) -> Self {
158 let word = ((status as u32) & 0x7)
159 | (((method_class as u32) & 0x1F) << 3)
160 | (((observer_role as u32) & 0x7) << 8)
161 | (((stability_class as u32) & 0x7) << 11)
162 | (((freshness_q10 as u32) & 0x3FF) << 14)
163 | (((confidence_q8 as u32) & 0xFF) << 24);
164 Self(word)
165 }
166
167 #[inline]
168 pub fn from_contextual(
169 tier: EpistemicTier,
170 method: ObservationMethod,
171 role: ObserverRole,
172 stability: TemporalStabilityClass,
173 freshness_q10: u16,
174 confidence_q8: u8,
175 ) -> Self {
176 let role_code: u8 = match role {
177 ObserverRole::SelfMeasured => 0,
178 ObserverRole::PeerMeasured => 1,
179 ObserverRole::ClinicianMeasured => 2,
180 ObserverRole::IsakCertified => 3,
181 ObserverRole::AutomatedSensor => 4,
182 ObserverRole::CuratorExtracted => 5,
183 };
184 let stability_code: u8 = match stability {
185 TemporalStabilityClass::AcutePhysiological => 0,
186 TemporalStabilityClass::SubacuteMetabolicGravidAndHealing => 1,
187 TemporalStabilityClass::OntogeneticAndSkeletalRemodeling => 2,
188 TemporalStabilityClass::SkeletallyMatureInvariant => 3,
189 };
190 Self::pack(
191 tier.rank(),
192 method.category_code(),
193 role_code,
194 stability_code,
195 freshness_q10,
196 confidence_q8,
197 )
198 }
199
200 #[inline]
201 pub const fn status(self) -> u8 {
202 (self.0 & 0x7) as u8
203 }
204
205 #[inline]
206 pub const fn method_class(self) -> u8 {
207 ((self.0 >> 3) & 0x1F) as u8
208 }
209
210 #[inline]
211 pub const fn observer_role(self) -> u8 {
212 ((self.0 >> 8) & 0x7) as u8
213 }
214
215 #[inline]
216 pub const fn stability_class(self) -> u8 {
217 ((self.0 >> 11) & 0x7) as u8
218 }
219
220 #[inline]
221 pub const fn freshness_q10(self) -> u16 {
222 ((self.0 >> 14) & 0x3FF) as u16
223 }
224
225 #[inline]
226 pub const fn confidence_q8(self) -> u8 {
227 ((self.0 >> 24) & 0xFF) as u8
228 }
229}
230
231#[repr(C)]
233#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
234pub struct Skl2JointDescriptorV1 {
235 pub joint_id: u64,
236 pub parent_index: u16,
237 pub loop_closure_partner: u16,
238 pub solver_regime: u8,
239 pub flags: u8,
240 pub reserved: u16,
241 pub swing_twist: SwingTwistConeLimits,
242}
243
244const _: () = assert!(core::mem::size_of::<Skl2JointDescriptorV1>() == 32);
245
246#[repr(C, align(64))]
248#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
249pub struct Skl2JointDescriptorV2 {
250 pub joint_id: u64,
251 pub parent_index: u16,
252 pub loop_closure_partner: u16,
253 pub landmark_index: u16,
254 pub actuation_pathway_index: u16,
255 pub swing_twist: SwingTwistConeLimits,
256 pub cortical_bmd_mg_cm3: f32,
257 pub trabecular_bv_tv: f32,
258 pub bone_length_m: f32,
259 pub segment_mass_kg: f32,
260 pub solver_regime: u8,
261 pub epistemic_tier: u8,
262 pub observation_method: u16,
263 pub confidence_q8: u8,
264 pub flags: u8,
265 pub reserved: u16,
266 pub provenance_assertion_idx: u32,
267 pub record_crc32c: u32,
268}
269
270const _: () = assert!(core::mem::size_of::<Skl2JointDescriptorV2>() == 64);
271const _: () = assert!(core::mem::align_of::<Skl2JointDescriptorV2>() == 64);
272
273impl Skl2JointDescriptorV2 {
274 #[allow(clippy::too_many_arguments)]
276 pub fn new(
277 joint_id: u64,
278 parent_index: u16,
279 swing_twist: SwingTwistConeLimits,
280 solver_regime: KinematicSolverRegime,
281 cortical_bmd_mg_cm3: f32,
282 trabecular_bv_tv: f32,
283 bone_length_m: f32,
284 segment_mass_kg: f32,
285 epistemic_tier: u8,
286 observation_method: u16,
287 confidence_q8: u8,
288 ) -> Self {
289 let desc = Self {
290 joint_id,
291 parent_index,
292 loop_closure_partner: SKL2_NONE_INDEX,
293 landmark_index: SKL2_NONE_INDEX,
294 actuation_pathway_index: SKL2_NONE_INDEX,
295 swing_twist,
296 cortical_bmd_mg_cm3,
297 trabecular_bv_tv,
298 bone_length_m,
299 segment_mass_kg,
300 solver_regime: solver_regime.as_u8(),
301 epistemic_tier,
302 observation_method,
303 confidence_q8,
304 flags: 0,
305 reserved: 0,
306 provenance_assertion_idx: 0,
307 record_crc32c: 0,
308 };
309 desc.with_sealed_crc()
310 }
311
312 #[inline]
313 pub fn compute_crc32c(&self) -> u32 {
314 let bytes = bytemuck::bytes_of(self);
315 crc32c(&bytes[..60])
316 }
317
318 #[inline]
319 pub fn seal_crc(&mut self) {
320 self.reserved = 0;
321 self.record_crc32c = self.compute_crc32c();
322 }
323
324 #[inline]
325 pub fn with_sealed_crc(mut self) -> Self {
326 self.seal_crc();
327 self
328 }
329
330 #[inline]
331 pub fn verify_crc(&self) -> bool {
332 self.record_crc32c == self.compute_crc32c()
333 }
334
335 #[inline]
336 pub const fn solver_regime(&self) -> Result<KinematicSolverRegime, SkeletalSectionError> {
337 KinematicSolverRegime::from_u8(self.solver_regime)
338 }
339
340 #[inline]
341 pub const fn epistemic_tier_enum(&self) -> Result<EpistemicTier, SkeletalSectionError> {
342 match self.epistemic_tier {
343 0 => Ok(EpistemicTier::DirectlyObserved),
344 1 => Ok(EpistemicTier::InferredFromObservation),
345 2 => Ok(EpistemicTier::ExtractedFromRecord),
346 3 => Ok(EpistemicTier::LongitudinallyProjected),
347 4 => Ok(EpistemicTier::StatisticallyCalibrated),
348 5 => Ok(EpistemicTier::PhylogeneticallyBracketed),
349 6 => Ok(EpistemicTier::AuthorialIntent),
350 _ => Err(SkeletalSectionError::InvalidData),
351 }
352 }
353
354 pub fn from_v1(v1: Skl2JointDescriptorV1) -> Result<Self, SkeletalSectionError> {
356 let _ = KinematicSolverRegime::from_u8(v1.solver_regime)?;
357 v1.swing_twist.validate()?;
358 let out = Self {
359 joint_id: v1.joint_id,
360 parent_index: v1.parent_index,
361 loop_closure_partner: v1.loop_closure_partner,
362 landmark_index: SKL2_NONE_INDEX,
363 actuation_pathway_index: SKL2_NONE_INDEX,
364 swing_twist: v1.swing_twist,
365 cortical_bmd_mg_cm3: 0.0,
366 trabecular_bv_tv: 0.0,
367 bone_length_m: 0.0,
368 segment_mass_kg: 0.0,
369 solver_regime: v1.solver_regime,
370 epistemic_tier: EpistemicTier::AuthorialIntent.rank(),
371 observation_method: 0,
372 confidence_q8: 0,
373 flags: v1.flags,
374 reserved: 0,
375 provenance_assertion_idx: 0,
376 record_crc32c: 0,
377 };
378 Ok(out.with_sealed_crc())
379 }
380
381 #[inline]
383 pub const fn to_v1(&self) -> Skl2JointDescriptorV1 {
384 Skl2JointDescriptorV1 {
385 joint_id: self.joint_id,
386 parent_index: self.parent_index,
387 loop_closure_partner: self.loop_closure_partner,
388 solver_regime: self.solver_regime,
389 flags: self.flags,
390 reserved: 0,
391 swing_twist: self.swing_twist,
392 }
393 }
394
395 pub fn validate(&self, joint_index: usize, joint_count: usize) -> Result<(), SkeletalSectionError> {
396 if self.reserved != 0 || !self.verify_crc() {
397 return Err(SkeletalSectionError::InvalidData);
398 }
399 let _ = self.solver_regime()?;
400 let _ = self.epistemic_tier_enum()?;
401 self.swing_twist.validate()?;
402 if !self.cortical_bmd_mg_cm3.is_finite()
403 || self.cortical_bmd_mg_cm3 < 0.0
404 || !self.trabecular_bv_tv.is_finite()
405 || !(0.0..=1.0).contains(&self.trabecular_bv_tv)
406 || !self.bone_length_m.is_finite()
407 || self.bone_length_m < 0.0
408 || !self.segment_mass_kg.is_finite()
409 || self.segment_mass_kg < 0.0
410 {
411 return Err(SkeletalSectionError::InvalidData);
412 }
413 if self.parent_index != SKL2_NONE_INDEX && usize::from(self.parent_index) >= joint_index {
414 return Err(SkeletalSectionError::InvalidData);
415 }
416 if self.loop_closure_partner != SKL2_NONE_INDEX
417 && usize::from(self.loop_closure_partner) >= joint_count
418 {
419 return Err(SkeletalSectionError::InvalidData);
420 }
421 Ok(())
422 }
423}
424
425#[repr(C)]
427#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
428pub struct LoopClosureConstraintRecord {
429 pub constraint_id: u64,
430 pub joint_a: u16,
431 pub joint_b: u16,
432 pub closure_type: u8,
433 pub flags: u8,
434 pub reserved: u16,
435 pub anchor_a_local: [f32; 3],
436 pub anchor_b_local: [f32; 3],
437 pub stiffness_n_m: f32,
438 pub damping_n_s_m: f32,
439 pub rest_angle_rad: f32,
440 pub tolerance_m: f32,
441 pub provenance: EpistemicProvenanceWord,
442 pub record_crc32c: u32,
443}
444
445const _: () = assert!(core::mem::size_of::<LoopClosureConstraintRecord>() == 64);
446
447impl LoopClosureConstraintRecord {
448 #[inline]
449 pub fn seal(&mut self) {
450 self.reserved = 0;
451 let bytes = bytemuck::bytes_of(self);
452 self.record_crc32c = crc32c(&bytes[..60]);
453 }
454
455 #[inline]
456 pub fn with_sealed_crc(mut self) -> Self {
457 self.seal();
458 self
459 }
460
461 #[inline]
462 pub fn verify_crc(&self) -> bool {
463 let bytes = bytemuck::bytes_of(self);
464 self.reserved == 0 && self.record_crc32c == crc32c(&bytes[..60])
465 }
466}
467
468#[repr(C)]
470#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
471pub struct TopologicalLandmarkRecord {
472 pub landmark_id: u64,
473 pub face_index: u32,
474 pub bary_u_q16: u16,
475 pub bary_v_q16: u16,
476 pub attached_joint: u16,
477 pub landmark_kind: u8,
478 pub flags: u8,
479 pub normal_offset_m: f32,
480 pub provenance: EpistemicProvenanceWord,
481 pub record_crc32c: u32,
482}
483
484const _: () = assert!(core::mem::size_of::<TopologicalLandmarkRecord>() == 32);
485
486impl TopologicalLandmarkRecord {
487 #[inline]
488 pub fn seal(&mut self) {
489 let bytes = bytemuck::bytes_of(self);
490 self.record_crc32c = crc32c(&bytes[..28]);
491 }
492
493 #[inline]
494 pub fn with_sealed_crc(mut self) -> Self {
495 self.seal();
496 self
497 }
498
499 #[inline]
500 pub fn verify_crc(&self) -> bool {
501 let bytes = bytemuck::bytes_of(self);
502 self.record_crc32c == crc32c(&bytes[..28])
503 }
504}
505
506#[repr(C)]
508#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
509pub struct ActuationPathwayRecord {
510 pub pathway_id: u64,
511 pub origin_joint: u16,
512 pub insertion_joint: u16,
513 pub pathway_kind: u8,
514 pub solver_regime: u8,
515 pub reserved: u16,
516 pub max_force_n: f32,
517 pub optimal_fiber_length_m: f32,
518 pub tendon_slack_length_m: f32,
519 pub pennation_angle_rad: f32,
520 pub moment_arm_m: f32,
521 pub activation_tau_s: f32,
522 pub provenance: EpistemicProvenanceWord,
523 pub record_crc32c: u32,
524}
525
526const _: () = assert!(core::mem::size_of::<ActuationPathwayRecord>() == 48);
527
528impl ActuationPathwayRecord {
529 #[inline]
530 pub fn seal(&mut self) {
531 self.reserved = 0;
532 let bytes = bytemuck::bytes_of(self);
533 self.record_crc32c = crc32c(&bytes[..44]);
534 }
535
536 #[inline]
537 pub fn with_sealed_crc(mut self) -> Self {
538 self.seal();
539 self
540 }
541
542 #[inline]
543 pub fn verify_crc(&self) -> bool {
544 let bytes = bytemuck::bytes_of(self);
545 self.reserved == 0 && self.record_crc32c == crc32c(&bytes[..44])
546 }
547}
548
549#[repr(C)]
551#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
552pub struct Ssm1BasisHeader {
553 pub magic: [u8; 4],
554 pub version: u16,
555 pub mode_count: u16,
556 pub target_vertex_count: u32,
557 pub target_joint_count: u16,
558 pub flags: u16,
559 pub species_taxon_hash: u64,
560 pub provenance: EpistemicProvenanceWord,
561 pub record_crc32c: u32,
562}
563
564const _: () = assert!(core::mem::size_of::<Ssm1BasisHeader>() == 32);
565
566impl Ssm1BasisHeader {
567 #[inline]
568 pub fn seal(&mut self) {
569 self.magic = SSM1_MAGIC;
570 let bytes = bytemuck::bytes_of(self);
571 self.record_crc32c = crc32c(&bytes[..28]);
572 }
573
574 #[inline]
575 pub fn with_sealed_crc(mut self) -> Self {
576 self.seal();
577 self
578 }
579
580 #[inline]
581 pub fn verify_crc(&self) -> bool {
582 let bytes = bytemuck::bytes_of(self);
583 self.magic == SSM1_MAGIC && self.record_crc32c == crc32c(&bytes[..28])
584 }
585}
586
587#[repr(C)]
589#[derive(Debug, Clone, Copy, PartialEq, Pod, Zeroable)]
590pub struct Ssm1ModeWeight {
591 pub mode_id: u32,
592 pub eigenvalue: f32,
593 pub min_sigma: f32,
594 pub max_sigma: f32,
595}
596
597const _: () = assert!(core::mem::size_of::<Ssm1ModeWeight>() == 16);
598
599#[derive(Debug, Clone, PartialEq)]
601pub struct SkeletalSectionPayload {
602 pub source_version: u16,
603 pub flags: u16,
604 pub joints: Vec<Skl2JointDescriptorV2>,
605 pub loop_closures: Vec<LoopClosureConstraintRecord>,
606 pub landmarks: Vec<TopologicalLandmarkRecord>,
607 pub actuation_pathways: Vec<ActuationPathwayRecord>,
608 pub ssm_headers: Vec<Ssm1BasisHeader>,
609 pub ssm_modes: Vec<Ssm1ModeWeight>,
610}
611
612impl SkeletalSectionPayload {
613 pub fn new(joints: Vec<Skl2JointDescriptorV2>) -> Self {
614 Self {
615 source_version: SKL2_VERSION_2,
616 flags: 0,
617 joints,
618 loop_closures: Vec::new(),
619 landmarks: Vec::new(),
620 actuation_pathways: Vec::new(),
621 ssm_headers: Vec::new(),
622 ssm_modes: Vec::new(),
623 }
624 }
625
626 pub fn validate(&self) -> Result<(), SkeletalSectionError> {
627 if self.joints.len() > SKL2_MAX_JOINTS {
628 return Err(SkeletalSectionError::CountLimit);
629 }
630 let jc = self.joints.len();
631 for (idx, j) in self.joints.iter().enumerate() {
632 j.validate(idx, jc)?;
633 if j.landmark_index != SKL2_NONE_INDEX
634 && usize::from(j.landmark_index) >= self.landmarks.len()
635 {
636 return Err(SkeletalSectionError::InvalidData);
637 }
638 if j.actuation_pathway_index != SKL2_NONE_INDEX
639 && usize::from(j.actuation_pathway_index) >= self.actuation_pathways.len()
640 {
641 return Err(SkeletalSectionError::InvalidData);
642 }
643 }
644 for lc in &self.loop_closures {
645 if !lc.verify_crc()
646 || usize::from(lc.joint_a) >= jc
647 || usize::from(lc.joint_b) >= jc
648 || lc.joint_a == lc.joint_b
649 {
650 return Err(SkeletalSectionError::InvalidData);
651 }
652 }
653 for lm in &self.landmarks {
654 if !lm.verify_crc()
655 || (lm.attached_joint != SKL2_NONE_INDEX && usize::from(lm.attached_joint) >= jc)
656 {
657 return Err(SkeletalSectionError::InvalidData);
658 }
659 }
660 for ap in &self.actuation_pathways {
661 if !ap.verify_crc()
662 || usize::from(ap.origin_joint) >= jc
663 || usize::from(ap.insertion_joint) >= jc
664 || KinematicSolverRegime::from_u8(ap.solver_regime).is_err()
665 {
666 return Err(SkeletalSectionError::InvalidData);
667 }
668 }
669 for sh in &self.ssm_headers {
670 if !sh.verify_crc() {
671 return Err(SkeletalSectionError::InvalidData);
672 }
673 }
674 if self.encoded_v2_size() > SKL2_MAX_CONTAINER_BYTES {
675 return Err(SkeletalSectionError::CountLimit);
676 }
677 Ok(())
678 }
679
680 #[inline]
681 pub fn encoded_v2_size(&self) -> usize {
682 self.encoded_body_size(SKL2_VERSION_2)
683 }
684
685 #[inline]
686 pub fn encoded_v1_size(&self) -> usize {
687 self.encoded_body_size(SKL2_VERSION_1)
688 }
689
690 fn encoded_body_size(&self, version: u16) -> usize {
691 let joint_stride = if version == SKL2_VERSION_1 { 32 } else { 64 };
692 SKL2_HEADER_LEN
693 + self.joints.len() * joint_stride
694 + self.loop_closures.len() * core::mem::size_of::<LoopClosureConstraintRecord>()
695 + self.landmarks.len() * core::mem::size_of::<TopologicalLandmarkRecord>()
696 + self.actuation_pathways.len() * core::mem::size_of::<ActuationPathwayRecord>()
697 + self.ssm_headers.len() * core::mem::size_of::<Ssm1BasisHeader>()
698 + self.ssm_modes.len() * core::mem::size_of::<Ssm1ModeWeight>()
699 }
700
701 pub fn encode_v2_into(&self, out: &mut [u8]) -> Result<usize, SkeletalSectionError> {
703 self.encode_into_version(SKL2_VERSION_2, out)
704 }
705
706 pub fn encode_v1_into(&self, out: &mut [u8]) -> Result<usize, SkeletalSectionError> {
708 self.encode_into_version(SKL2_VERSION_1, out)
709 }
710
711 pub fn to_bytes_v2(&self) -> Result<Vec<u8>, SkeletalSectionError> {
713 let len = self.encoded_v2_size();
714 let mut buf = vec![0u8; len];
715 let written = self.encode_v2_into(&mut buf)?;
716 buf.truncate(written);
717 Ok(buf)
718 }
719
720 pub fn to_bytes_v1(&self) -> Result<Vec<u8>, SkeletalSectionError> {
722 let len = self.encoded_v1_size();
723 let mut buf = vec![0u8; len];
724 let written = self.encode_v1_into(&mut buf)?;
725 buf.truncate(written);
726 Ok(buf)
727 }
728
729 fn encode_into_version(
730 &self,
731 version: u16,
732 out: &mut [u8],
733 ) -> Result<usize, SkeletalSectionError> {
734 self.validate()?;
735 let total_len = self.encoded_body_size(version);
736 if total_len > SKL2_MAX_CONTAINER_BYTES {
737 return Err(SkeletalSectionError::CountLimit);
738 }
739 if out.len() < total_len {
740 return Err(SkeletalSectionError::OutputTooSmall);
741 }
742
743 let mut cursor = SKL2_HEADER_LEN;
744 if version == SKL2_VERSION_1 {
745 for j in &self.joints {
746 let v1 = j.to_v1();
747 out[cursor..cursor + 32].copy_from_slice(bytemuck::bytes_of(&v1));
748 cursor += 32;
749 }
750 } else {
751 for j in &self.joints {
752 let mut sealed = *j;
753 sealed.seal_crc();
754 out[cursor..cursor + 64].copy_from_slice(bytemuck::bytes_of(&sealed));
755 cursor += 64;
756 }
757 }
758
759 for lc in &self.loop_closures {
760 let mut sealed = *lc;
761 sealed.seal();
762 out[cursor..cursor + 64].copy_from_slice(bytemuck::bytes_of(&sealed));
763 cursor += 64;
764 }
765 for lm in &self.landmarks {
766 let mut sealed = *lm;
767 sealed.seal();
768 out[cursor..cursor + 32].copy_from_slice(bytemuck::bytes_of(&sealed));
769 cursor += 32;
770 }
771 for ap in &self.actuation_pathways {
772 let mut sealed = *ap;
773 sealed.seal();
774 out[cursor..cursor + 48].copy_from_slice(bytemuck::bytes_of(&sealed));
775 cursor += 48;
776 }
777 for sh in &self.ssm_headers {
778 let mut sealed = *sh;
779 sealed.seal();
780 out[cursor..cursor + 32].copy_from_slice(bytemuck::bytes_of(&sealed));
781 cursor += 32;
782 }
783 for sm in &self.ssm_modes {
784 out[cursor..cursor + 16].copy_from_slice(bytemuck::bytes_of(sm));
785 cursor += 16;
786 }
787
788 let payload_crc = crc32c(&out[SKL2_HEADER_LEN..cursor]);
789
790 out[0..4].copy_from_slice(&SKL2_MAGIC);
791 out[4..6].copy_from_slice(&version.to_le_bytes());
792 out[6..8].copy_from_slice(&self.flags.to_le_bytes());
793 out[8..10].copy_from_slice(&(self.joints.len() as u16).to_le_bytes());
794 out[10..12].copy_from_slice(&(self.loop_closures.len() as u16).to_le_bytes());
795 out[12..14].copy_from_slice(&(self.landmarks.len() as u16).to_le_bytes());
796 out[14..16].copy_from_slice(&(self.actuation_pathways.len() as u16).to_le_bytes());
797 out[16..18].copy_from_slice(&(self.ssm_headers.len() as u16).to_le_bytes());
798 out[18..20].copy_from_slice(&(self.ssm_modes.len() as u16).to_le_bytes());
799 out[20..24].copy_from_slice(&0u32.to_le_bytes());
800 out[24..28].copy_from_slice(&payload_crc.to_le_bytes());
801 let header_crc = crc32c(&out[0..28]);
802 out[28..32].copy_from_slice(&header_crc.to_le_bytes());
803
804 Ok(cursor)
805 }
806
807 pub fn from_bytes(bytes: &[u8]) -> Result<Self, SkeletalSectionError> {
809 if bytes.len() > SKL2_MAX_CONTAINER_BYTES {
810 return Err(SkeletalSectionError::CountLimit);
811 }
812 if bytes.len() < SKL2_HEADER_LEN || bytes[0..4] != SKL2_MAGIC {
813 return Err(SkeletalSectionError::Header);
814 }
815 let version = u16::from_le_bytes([bytes[4], bytes[5]]);
816 if version != SKL2_VERSION_1 && version != SKL2_VERSION_2 {
817 return Err(SkeletalSectionError::Header);
818 }
819 let expected_header_crc = u32::from_le_bytes([bytes[28], bytes[29], bytes[30], bytes[31]]);
820 if crc32c(&bytes[0..28]) != expected_header_crc {
821 return Err(SkeletalSectionError::Header);
822 }
823 let reserved = u32::from_le_bytes([bytes[20], bytes[21], bytes[22], bytes[23]]);
824 if reserved != 0 {
825 return Err(SkeletalSectionError::Header);
826 }
827
828 let flags = u16::from_le_bytes([bytes[6], bytes[7]]);
829 let joint_count = u16::from_le_bytes([bytes[8], bytes[9]]) as usize;
830 let loop_closure_count = u16::from_le_bytes([bytes[10], bytes[11]]) as usize;
831 let landmark_count = u16::from_le_bytes([bytes[12], bytes[13]]) as usize;
832 let actuation_pathway_count = u16::from_le_bytes([bytes[14], bytes[15]]) as usize;
833 let ssm_header_count = u16::from_le_bytes([bytes[16], bytes[17]]) as usize;
834 let ssm_mode_count = u16::from_le_bytes([bytes[18], bytes[19]]) as usize;
835 let expected_payload_crc = u32::from_le_bytes([bytes[24], bytes[25], bytes[26], bytes[27]]);
836
837 if joint_count > SKL2_MAX_JOINTS {
838 return Err(SkeletalSectionError::CountLimit);
839 }
840
841 let joint_stride = if version == SKL2_VERSION_1 { 32 } else { 64 };
842 let expected_len = SKL2_HEADER_LEN
843 .checked_add(joint_count * joint_stride)
844 .and_then(|v| v.checked_add(loop_closure_count * 64))
845 .and_then(|v| v.checked_add(landmark_count * 32))
846 .and_then(|v| v.checked_add(actuation_pathway_count * 48))
847 .and_then(|v| v.checked_add(ssm_header_count * 32))
848 .and_then(|v| v.checked_add(ssm_mode_count * 16))
849 .ok_or(SkeletalSectionError::Length)?;
850
851 if bytes.len() != expected_len {
852 return Err(SkeletalSectionError::Length);
853 }
854 if crc32c(&bytes[SKL2_HEADER_LEN..]) != expected_payload_crc {
855 return Err(SkeletalSectionError::InvalidData);
856 }
857
858 let mut cursor = SKL2_HEADER_LEN;
859 let mut joints = Vec::with_capacity(joint_count);
860 if version == SKL2_VERSION_1 {
861 for _ in 0..joint_count {
862 let mut v1 = Skl2JointDescriptorV1::zeroed();
863 bytemuck::bytes_of_mut(&mut v1).copy_from_slice(&bytes[cursor..cursor + 32]);
864 cursor += 32;
865 if v1.reserved != 0 {
866 return Err(SkeletalSectionError::InvalidData);
867 }
868 joints.push(Skl2JointDescriptorV2::from_v1(v1)?);
869 }
870 } else {
871 for _ in 0..joint_count {
872 let mut v2 = Skl2JointDescriptorV2::zeroed();
873 bytemuck::bytes_of_mut(&mut v2).copy_from_slice(&bytes[cursor..cursor + 64]);
874 cursor += 64;
875 joints.push(v2);
876 }
877 }
878
879 let mut loop_closures = Vec::with_capacity(loop_closure_count);
880 for _ in 0..loop_closure_count {
881 let mut lc = LoopClosureConstraintRecord::zeroed();
882 bytemuck::bytes_of_mut(&mut lc).copy_from_slice(&bytes[cursor..cursor + 64]);
883 cursor += 64;
884 loop_closures.push(lc);
885 }
886
887 let mut landmarks = Vec::with_capacity(landmark_count);
888 for _ in 0..landmark_count {
889 let mut lm = TopologicalLandmarkRecord::zeroed();
890 bytemuck::bytes_of_mut(&mut lm).copy_from_slice(&bytes[cursor..cursor + 32]);
891 cursor += 32;
892 landmarks.push(lm);
893 }
894
895 let mut actuation_pathways = Vec::with_capacity(actuation_pathway_count);
896 for _ in 0..actuation_pathway_count {
897 let mut ap = ActuationPathwayRecord::zeroed();
898 bytemuck::bytes_of_mut(&mut ap).copy_from_slice(&bytes[cursor..cursor + 48]);
899 cursor += 48;
900 actuation_pathways.push(ap);
901 }
902
903 let mut ssm_headers = Vec::with_capacity(ssm_header_count);
904 for _ in 0..ssm_header_count {
905 let mut sh = Ssm1BasisHeader::zeroed();
906 bytemuck::bytes_of_mut(&mut sh).copy_from_slice(&bytes[cursor..cursor + 32]);
907 cursor += 32;
908 ssm_headers.push(sh);
909 }
910
911 let mut ssm_modes = Vec::with_capacity(ssm_mode_count);
912 for _ in 0..ssm_mode_count {
913 let mut sm = Ssm1ModeWeight::zeroed();
914 bytemuck::bytes_of_mut(&mut sm).copy_from_slice(&bytes[cursor..cursor + 16]);
915 cursor += 16;
916 ssm_modes.push(sm);
917 }
918
919 let payload = Self {
920 source_version: version,
921 flags,
922 joints,
923 loop_closures,
924 landmarks,
925 actuation_pathways,
926 ssm_headers,
927 ssm_modes,
928 };
929 payload.validate()?;
930 Ok(payload)
931 }
932}
933
934#[cfg(test)]
935mod tests {
936 use super::*;
937
938 fn sample_v2_payload() -> SkeletalSectionPayload {
939 let j0 = Skl2JointDescriptorV2::new(
940 0x1000,
941 SKL2_NONE_INDEX,
942 SwingTwistConeLimits::symmetric(0.35, 0.2),
943 KinematicSolverRegime::ArticulatedRigidBodyDca,
944 1150.0,
945 0.32,
946 0.25,
947 12.5,
948 EpistemicTier::DirectlyObserved.rank(),
949 ObservationMethod::MicroCtSynchrotron as u16,
950 245,
951 );
952 let mut j1 = Skl2JointDescriptorV2::new(
953 0x1001,
954 0,
955 SwingTwistConeLimits::new(0.8, 0.6, -0.4, 0.4),
956 KinematicSolverRegime::CosseratRodContinuum,
957 980.0,
958 0.28,
959 0.40,
960 4.2,
961 EpistemicTier::InferredFromObservation.rank(),
962 ObservationMethod::OpticalMarkerMoCap as u16,
963 210,
964 );
965 j1.landmark_index = 0;
966 j1.actuation_pathway_index = 0;
967 j1.seal_crc();
968
969 let lc = LoopClosureConstraintRecord {
970 constraint_id: 0x2000,
971 joint_a: 0,
972 joint_b: 1,
973 closure_type: 1,
974 flags: 0,
975 reserved: 0,
976 anchor_a_local: [0.0, 0.1, 0.0],
977 anchor_b_local: [0.0, -0.1, 0.0],
978 stiffness_n_m: 1500.0,
979 damping_n_s_m: 45.0,
980 rest_angle_rad: 0.0,
981 tolerance_m: 0.002,
982 provenance: EpistemicProvenanceWord::pack(0, 1, 0, 0, 1000, 240),
983 record_crc32c: 0,
984 }
985 .with_sealed_crc();
986
987 let lm = TopologicalLandmarkRecord {
988 landmark_id: 0x3000,
989 face_index: 42,
990 bary_u_q16: 21845,
991 bary_v_q16: 21845,
992 attached_joint: 1,
993 landmark_kind: 2,
994 flags: 0,
995 normal_offset_m: 0.004,
996 provenance: EpistemicProvenanceWord::pack(1, 2, 1, 0, 900, 220),
997 record_crc32c: 0,
998 }
999 .with_sealed_crc();
1000
1001 let ap = ActuationPathwayRecord {
1002 pathway_id: 0x4000,
1003 origin_joint: 0,
1004 insertion_joint: 1,
1005 pathway_kind: 1,
1006 solver_regime: KinematicSolverRegime::ArticulatedRigidBodyDca.as_u8(),
1007 reserved: 0,
1008 max_force_n: 850.0,
1009 optimal_fiber_length_m: 0.18,
1010 tendon_slack_length_m: 0.12,
1011 pennation_angle_rad: 0.22,
1012 moment_arm_m: 0.035,
1013 activation_tau_s: 0.015,
1014 provenance: EpistemicProvenanceWord::pack(2, 1, 1, 2, 750, 190),
1015 record_crc32c: 0,
1016 }
1017 .with_sealed_crc();
1018
1019 let sh = Ssm1BasisHeader {
1020 magic: SSM1_MAGIC,
1021 version: 1,
1022 mode_count: 1,
1023 target_vertex_count: 4096,
1024 target_joint_count: 24,
1025 flags: 0,
1026 species_taxon_hash: 0xCAFE_BABE_1234_5678,
1027 provenance: EpistemicProvenanceWord::pack(4, 3, 4, 0, 1023, 230),
1028 record_crc32c: 0,
1029 }
1030 .with_sealed_crc();
1031
1032 let sm = Ssm1ModeWeight {
1033 mode_id: 0,
1034 eigenvalue: 0.42,
1035 min_sigma: -3.0,
1036 max_sigma: 3.0,
1037 };
1038
1039 SkeletalSectionPayload {
1040 source_version: SKL2_VERSION_2,
1041 flags: 0,
1042 joints: vec![j0, j1],
1043 loop_closures: vec![lc],
1044 landmarks: vec![lm],
1045 actuation_pathways: vec![ap],
1046 ssm_headers: vec![sh],
1047 ssm_modes: vec![sm],
1048 }
1049 }
1050
1051 #[test]
1052 fn skl2_v2_descriptor_layout_and_roundtrip() {
1053 assert_eq!(core::mem::size_of::<Skl2JointDescriptorV1>(), 32);
1054 assert_eq!(core::mem::size_of::<Skl2JointDescriptorV2>(), 64);
1055 assert_eq!(core::mem::align_of::<Skl2JointDescriptorV2>(), 64);
1056
1057 let payload = sample_v2_payload();
1058 let bytes = payload.to_bytes_v2().expect("v2 encode");
1059 assert_eq!(&bytes[0..4], b"SKL2");
1060
1061 let decoded = SkeletalSectionPayload::from_bytes(&bytes).expect("v2 decode");
1062 assert_eq!(decoded.source_version, SKL2_VERSION_2);
1063 assert_eq!(decoded.joints, payload.joints);
1064 assert_eq!(decoded.loop_closures, payload.loop_closures);
1065 assert_eq!(decoded.landmarks, payload.landmarks);
1066 assert_eq!(decoded.actuation_pathways, payload.actuation_pathways);
1067 assert_eq!(decoded.ssm_headers, payload.ssm_headers);
1068 assert_eq!(decoded.ssm_modes, payload.ssm_modes);
1069 }
1070
1071 #[test]
1072 fn skl2_v1_lossless_upgrade_decode() {
1073 let mut payload = sample_v2_payload();
1074 payload.joints[1].landmark_index = SKL2_NONE_INDEX;
1075 payload.joints[1].actuation_pathway_index = SKL2_NONE_INDEX;
1076 payload.loop_closures.clear();
1077 payload.landmarks.clear();
1078 payload.actuation_pathways.clear();
1079 payload.ssm_headers.clear();
1080 payload.ssm_modes.clear();
1081
1082 let v1_bytes = payload.to_bytes_v1().expect("v1 encode");
1083 assert_eq!(
1084 v1_bytes.len(),
1085 SKL2_HEADER_LEN + payload.joints.len() * 32
1086 );
1087
1088 let decoded = SkeletalSectionPayload::from_bytes(&v1_bytes).expect("v1 decode");
1089 assert_eq!(decoded.source_version, SKL2_VERSION_1);
1090 assert_eq!(decoded.joints.len(), 2);
1091 assert_eq!(decoded.joints[0].joint_id, 0x1000);
1092 assert_eq!(decoded.joints[1].joint_id, 0x1001);
1093 assert_eq!(
1094 decoded.joints[1].solver_regime().unwrap(),
1095 KinematicSolverRegime::CosseratRodContinuum
1096 );
1097 assert_eq!(decoded.joints[1].swing_twist, payload.joints[1].swing_twist);
1098 assert!(decoded.joints[0].verify_crc());
1099 assert!(decoded.joints[1].verify_crc());
1100 }
1101
1102 #[test]
1103 fn skl2_corrupted_crc_fails_closed() {
1104 let payload = sample_v2_payload();
1105 let mut bytes = payload.to_bytes_v2().expect("v2 encode");
1106 bytes[SKL2_HEADER_LEN + 8] ^= 0xFF;
1108 assert_eq!(
1109 SkeletalSectionPayload::from_bytes(&bytes),
1110 Err(SkeletalSectionError::InvalidData)
1111 );
1112 }
1113
1114 #[test]
1115 fn epistemic_provenance_word_bit_pack_roundtrip() {
1116 let word = EpistemicProvenanceWord::from_contextual(
1117 EpistemicTier::PhylogeneticallyBracketed,
1118 ObservationMethod::PublishedScientificLiterature,
1119 ObserverRole::PeerMeasured,
1120 TemporalStabilityClass::OntogeneticAndSkeletalRemodeling,
1121 777,
1122 213,
1123 );
1124 assert_eq!(word.status(), EpistemicTier::PhylogeneticallyBracketed.rank());
1125 assert_eq!(
1126 word.method_class(),
1127 ObservationMethod::PublishedScientificLiterature.category_code()
1128 );
1129 assert_eq!(word.observer_role(), 1);
1130 assert_eq!(word.stability_class(), 2);
1131 assert_eq!(word.freshness_q10(), 777);
1132 assert_eq!(word.confidence_q8(), 213);
1133 }
1134
1135 #[test]
1136 fn zero_heap_encode_into_caller_buffer() {
1137 let payload = sample_v2_payload();
1138 let needed = payload.encoded_v2_size();
1139 let mut short = vec![0u8; needed - 1];
1140 assert_eq!(
1141 payload.encode_v2_into(&mut short),
1142 Err(SkeletalSectionError::OutputTooSmall)
1143 );
1144 let mut exact = vec![0u8; needed];
1145 let written = payload.encode_v2_into(&mut exact).expect("encode into exact");
1146 assert_eq!(written, needed);
1147 let decoded = SkeletalSectionPayload::from_bytes(&exact[..written]).expect("decode");
1148 assert_eq!(decoded.joints, payload.joints);
1149 }
1150}