1use super::envelope::*;
9use super::types::*;
10
11pub const HAPTIC_RECORD_STRIDE: u32 = 64;
13
14pub const HAPTIC_VALID_FORCE: u32 = 1 << 0;
16pub const HAPTIC_VALID_INDENTATION: u32 = 1 << 1;
18pub const HAPTIC_VALID_DISPLACEMENT: u32 = 1 << 2;
20pub const HAPTIC_VALID_CARRIER_HZ: u32 = 1 << 3;
22pub const HAPTIC_VALID_ALL: u32 =
24 HAPTIC_VALID_FORCE | HAPTIC_VALID_INDENTATION | HAPTIC_VALID_DISPLACEMENT | HAPTIC_VALID_CARRIER_HZ;
25
26#[derive(Debug, Clone, PartialEq)]
28pub enum HapticRole {
29 Measurement {
31 receptor_model_uri: Option<String>,
32 },
33 ReceptorModel {
35 receptor_model_uri: String,
36 },
37 ActuatorCommand {
39 device_profile_digest: [u8; 32],
40 authority_receipt_digest: [u8; 32],
41 command_expiry: InstantSpec,
42 receptor_model_uri: Option<String>,
43 },
44}
45
46impl HapticRole {
47 pub fn as_str(&self) -> &'static str {
48 match self {
49 Self::Measurement { .. } => "measurement",
50 Self::ReceptorModel { .. } => "receptor-model",
51 Self::ActuatorCommand { .. } => "actuator-command",
52 }
53 }
54}
55
56#[derive(Debug, Clone, Copy, PartialEq, Eq)]
58pub enum ForceActionConvention {
59 ForceOnBody,
61 ForceOnDevice,
63}
64
65impl ForceActionConvention {
66 pub const fn to_force_on_body(self, f: [f64; 3]) -> [f64; 3] {
68 match self {
69 Self::ForceOnBody => f,
70 Self::ForceOnDevice => [-f[0], -f[1], -f[2]],
71 }
72 }
73}
74
75#[derive(Debug, Clone, PartialEq)]
77pub struct HapticContactSpec {
78 pub role: HapticRole,
79 pub contact_frame: FrameSpec<'static>,
80 pub contact_area_m2: Option<f64>,
81}
82
83#[derive(Debug, Clone, Copy, PartialEq)]
85pub struct HapticRecord {
86 pub contact_position_m: [f32; 3],
87 pub contact_area_m2: f32,
88 pub force_local_xyz_n: [f32; 3],
89 pub normal_indentation_m: f32,
90 pub displacement_local_xyz_m: [f32; 3],
91 pub carrier_hz: f32,
92 pub validity_bits: u32,
93 pub contact_ref: u32,
94}
95
96impl HapticRecord {
97 pub fn validate(&self, externally_valid: bool) -> Result<(), ProfileError> {
98 for v in self.contact_position_m {
99 if !v.is_finite() {
100 return Err(ProfileError::NonFiniteFloat);
101 }
102 require_zero_when_masked(externally_valid, v)?;
103 }
104 if !self.contact_area_m2.is_finite() || self.contact_area_m2 < 0.0 {
105 return Err(ProfileError::InvalidMetadata("haptic contact_area_m2 < 0"));
106 }
107 require_zero_when_masked(externally_valid, self.contact_area_m2)?;
108 for v in self
109 .force_local_xyz_n
110 .iter()
111 .chain(std::iter::once(&self.normal_indentation_m))
112 .chain(self.displacement_local_xyz_m.iter())
113 .chain(std::iter::once(&self.carrier_hz))
114 {
115 if !v.is_finite() {
116 return Err(ProfileError::NonFiniteFloat);
117 }
118 }
119 if self.validity_bits & !HAPTIC_VALID_ALL != 0 {
120 return Err(ProfileError::InvalidValidityBits);
121 }
122 if self.contact_ref == 0 {
123 return Err(ProfileError::InvalidMetadata("haptic contact_ref must be nonzero Schema 207 ID"));
124 }
125 if !externally_valid && self.validity_bits != 0 {
126 return Err(ProfileError::MaskedLaneNonZero);
127 }
128
129 let force_valid = externally_valid && (self.validity_bits & HAPTIC_VALID_FORCE != 0);
130 let ind_valid = externally_valid && (self.validity_bits & HAPTIC_VALID_INDENTATION != 0);
131 let disp_valid = externally_valid && (self.validity_bits & HAPTIC_VALID_DISPLACEMENT != 0);
132 let carrier_valid = externally_valid && (self.validity_bits & HAPTIC_VALID_CARRIER_HZ != 0);
133
134 require_zero_vec3_when_masked(force_valid, self.force_local_xyz_n)?;
135 require_zero_when_masked(ind_valid, self.normal_indentation_m)?;
136 require_zero_vec3_when_masked(disp_valid, self.displacement_local_xyz_m)?;
137 require_zero_when_masked(carrier_valid, self.carrier_hz)?;
138 if carrier_valid && self.carrier_hz < 0.0 {
139 return Err(ProfileError::InvalidMetadata("negative haptic carrier_hz"));
140 }
141 Ok(())
142 }
143
144 pub fn decode(bytes: &[u8], externally_valid: bool) -> Result<Self, ProfileError> {
145 if bytes.len() != HAPTIC_RECORD_STRIDE as usize {
146 return Err(ProfileError::InvalidPayloadLength);
147 }
148 ensure_zero_bytes(&bytes[56..64])?;
149 let rec = Self {
150 contact_position_m: read_f32_3(bytes, 0)?,
151 contact_area_m2: read_f32_finite(bytes, 12)?,
152 force_local_xyz_n: read_f32_3(bytes, 16)?,
153 normal_indentation_m: read_f32_finite(bytes, 28)?,
154 displacement_local_xyz_m: read_f32_3(bytes, 32)?,
155 carrier_hz: read_f32_finite(bytes, 44)?,
156 validity_bits: read_u32(bytes, 48)?,
157 contact_ref: read_u32(bytes, 52)?,
158 };
159 rec.validate(externally_valid)?;
160 Ok(rec)
161 }
162
163 pub fn encode_into(&self, out: &mut [u8], externally_valid: bool) -> Result<(), ProfileError> {
164 if out.len() != HAPTIC_RECORD_STRIDE as usize {
165 return Err(ProfileError::InvalidPayloadLength);
166 }
167 self.validate(externally_valid)?;
168 out.fill(0);
169 write_f32_slice(out, 0, &self.contact_position_m)?;
170 write_f32_finite(out, 12, self.contact_area_m2)?;
171 write_f32_slice(out, 16, &self.force_local_xyz_n)?;
172 write_f32_finite(out, 28, self.normal_indentation_m)?;
173 write_f32_slice(out, 32, &self.displacement_local_xyz_m)?;
174 write_f32_finite(out, 44, self.carrier_hz)?;
175 write_u32(out, 48, self.validity_bits);
176 write_u32(out, 52, self.contact_ref);
177 Ok(())
178 }
179}
180
181pub struct HapticChunkView<'a> {
183 bytes: &'a [u8],
184 mask: Option<&'a [u8]>,
185}
186
187impl<'a> HapticChunkView<'a> {
188 pub fn len(&self) -> usize {
189 self.bytes.len() / HAPTIC_RECORD_STRIDE as usize
190 }
191
192 pub fn is_empty(&self) -> bool {
193 self.bytes.is_empty()
194 }
195
196 pub fn get(&self, index: usize) -> Result<HapticRecord, ProfileError> {
197 let start = index
198 .checked_mul(HAPTIC_RECORD_STRIDE as usize)
199 .ok_or(ProfileError::Overflow)?;
200 let end = start
201 .checked_add(HAPTIC_RECORD_STRIDE as usize)
202 .ok_or(ProfileError::Overflow)?;
203 let slice = self
204 .bytes
205 .get(start..end)
206 .ok_or(ProfileError::OutOfBoundsIndex("haptic record index"))?;
207 let ext_valid = self.mask.map_or(true, |m| m[index] != 0);
208 HapticRecord::decode(slice, ext_valid)
209 }
210}
211
212pub fn rotate_force_to_parent(
218 force_local_xyz_n: [f32; 3],
219 contact_rotation: QuaternionXyzw,
220 convention: ForceActionConvention,
221) -> Result<[f64; 3], ProfileError> {
222 for c in force_local_xyz_n {
223 if !c.is_finite() {
224 return Err(ProfileError::NonFiniteFloat);
225 }
226 }
227 let body_local = convention.to_force_on_body([
228 f64::from(force_local_xyz_n[0]),
229 f64::from(force_local_xyz_n[1]),
230 f64::from(force_local_xyz_n[2]),
231 ]);
232 Ok(contact_rotation.rotate_vec3(body_local))
233}
234
235pub fn torque_about_origin_n_m(position_m: [f64; 3], force_n: [f64; 3]) -> Result<[f64; 3], ProfileError> {
237 if position_m.iter().chain(force_n.iter()).any(|v| !v.is_finite()) {
238 return Err(ProfileError::NonFiniteFloat);
239 }
240 Ok([
241 position_m[1] * force_n[2] - position_m[2] * force_n[1],
242 position_m[2] * force_n[0] - position_m[0] * force_n[2],
243 position_m[0] * force_n[1] - position_m[1] * force_n[0],
244 ])
245}
246
247pub fn contact_pressure_pa(normal_force_n: f64, contact_area_m2: f64) -> Result<f64, ProfileError> {
249 if !normal_force_n.is_finite() || !contact_area_m2.is_finite() {
250 return Err(ProfileError::NonFiniteFloat);
251 }
252 if contact_area_m2 <= 0.0 {
253 return Err(ProfileError::InvalidMetadata("contact_area_m2 must be > 0 to derive pressure"));
254 }
255 Ok(normal_force_n / contact_area_m2)
256}
257
258pub fn hooke_spring_force_n(
260 stiffness_n_per_m: f64,
261 displacement_m: f64,
262 preload_n: f64,
263) -> Result<f64, ProfileError> {
264 if !stiffness_n_per_m.is_finite() || !displacement_m.is_finite() || !preload_n.is_finite() {
265 return Err(ProfileError::NonFiniteFloat);
266 }
267 if stiffness_n_per_m < 0.0 {
268 return Err(ProfileError::InvalidMetadata("spring stiffness < 0"));
269 }
270 Ok(stiffness_n_per_m * displacement_m + preload_n)
271}
272
273pub fn write_haptic_qac(
282 records: &[HapticRecord],
283 contact_spec: &HapticContactSpec,
284 receptor_output: Option<&[f32]>,
285 clock: &ClockSpec,
286 sample_rate: RationalRate,
287 frame: &FrameSpec<'_>,
288 provenance: &ProvenanceSpec<'_>,
289 validity_mask: Option<&[u8]>,
290 limits: ProfileLimits,
291) -> Result<Vec<u8>, ProfileError> {
292 if records.is_empty() {
293 return Err(ProfileError::InvalidPayloadLength);
294 }
295 if let Some(m) = validity_mask {
296 if m.len() != records.len() || m.iter().any(|&b| b > 1) {
297 return Err(ProfileError::InvalidValidityBits);
298 }
299 }
300 if let Some(ro) = receptor_output {
301 if ro.len() != records.len() {
302 return Err(ProfileError::InvalidPayloadLength);
303 }
304 }
305 if let Some(area) = contact_spec.contact_area_m2 {
306 if !area.is_finite() || area <= 0.0 {
307 return Err(ProfileError::InvalidMetadata("Schema 207 contact_area_m2 must be > 0"));
308 }
309 }
310
311 const CONTACT_BLOCK_ID: u32 = 4;
313 let mut payload = vec![0u8; records.len() * HAPTIC_RECORD_STRIDE as usize];
314 for (idx, r) in records.iter().enumerate() {
315 if r.contact_ref != CONTACT_BLOCK_ID {
316 return Err(ProfileError::InvalidMetadata("HapticRecord.contact_ref must match Schema 207 ID 4"));
317 }
318 let ext_valid = validity_mask.map_or(true, |m| m[idx] != 0);
319 let off = idx * HAPTIC_RECORD_STRIDE as usize;
320 r.encode_into(&mut payload[off..off + HAPTIC_RECORD_STRIDE as usize], ext_valid)?;
321 }
322
323 let mut meta = MetadataBuilder::new();
324 meta.push_clock(1, clock)?;
325 meta.push_frame(2, frame)?;
326 meta.push_provenance(3, provenance)?;
327 meta.push_frame(12, &contact_spec.contact_frame)?;
329
330 let mut s207_fields = vec![
331 OwnedField::crit(1, OwnedValue::Utf8(contact_spec.role.as_str().to_owned())),
332 OwnedField::crit(2, OwnedValue::MetadataRefs(vec![12])),
333 ];
334 if let Some(area) = contact_spec.contact_area_m2 {
335 s207_fields.push(OwnedField::opt(3, OwnedValue::F64(vec![area])));
336 }
337 match &contact_spec.role {
338 HapticRole::Measurement { receptor_model_uri } => {
339 if let Some(uri) = receptor_model_uri {
340 validate_identifier_str(uri, "receptor_model_URI")?;
341 s207_fields.push(OwnedField::opt(4, OwnedValue::Utf8(uri.clone())));
342 }
343 }
344 HapticRole::ReceptorModel { receptor_model_uri } => {
345 validate_identifier_str(receptor_model_uri, "receptor_model_URI")?;
346 s207_fields.push(OwnedField::crit(4, OwnedValue::Utf8(receptor_model_uri.clone())));
347 }
348 HapticRole::ActuatorCommand {
349 device_profile_digest,
350 authority_receipt_digest,
351 command_expiry,
352 receptor_model_uri,
353 } => {
354 if command_expiry.clock_block_id != 1 {
355 return Err(ProfileError::InvalidMetadata("command_expiry must reference clock block 1"));
356 }
357 if let Some(uri) = receptor_model_uri {
358 validate_identifier_str(uri, "receptor_model_URI")?;
359 s207_fields.push(OwnedField::opt(4, OwnedValue::Utf8(uri.clone())));
360 }
361 meta.push_instant(8, command_expiry)?;
362 s207_fields.push(OwnedField::crit(5, OwnedValue::Bytes(device_profile_digest.to_vec())));
363 s207_fields.push(OwnedField::crit(6, OwnedValue::Bytes(authority_receipt_digest.to_vec())));
364 s207_fields.push(OwnedField::crit(7, OwnedValue::MetadataRefs(vec![8])));
365 }
366 }
367
368 let receptor_entry_idx = if receptor_output.is_some() {
369 Some(if validity_mask.is_some() { 3u32 } else { 2u32 })
370 } else {
371 None
372 };
373 if let Some(ei) = receptor_entry_idx {
374 s207_fields.push(OwnedField::opt(9, OwnedValue::EntryRefs(vec![ei])));
375 }
376 meta.push_block(OwnedBlock {
377 id: CONTACT_BLOCK_ID,
378 schema_type: 207,
379 revision: 1,
380 critical: true,
381 fields: s207_fields,
382 })?;
383
384 let shape = [records.len() as u64];
385 meta.push_model_grid(
386 5,
387 &ModelGridSpec {
388 model_uri: "urn:qac:model:haptic-mechanical-sample:v1",
389 model_version: "1",
390 discretization: None,
391 component_offsets_xyz: &[],
392 component_time_offset_num: &[],
393 component_time_offset_den: &[],
394 timestep: Some(sample_rate),
395 tolerance_abs: None,
396 tolerance_rel: None,
397 tensor_shape: &shape,
398 component_names: "contact_xyz,area,force_xyz,indentation,disp_xyz,carrier_hz",
399 },
400 )?;
401
402 let valid_count = validity_mask.map(|m| m.iter().filter(|&&b| b == 1).count() as u64);
403 let mask_entry_idx = if validity_mask.is_some() { Some(2u32) } else { None };
404 let ext = [CONTACT_BLOCK_ID, 5u32];
405 meta.push_descriptor(
406 6,
407 &DescriptorSpec {
408 clock_id: Some(1),
409 frame_id: Some(2),
410 provenance_id: 3,
411 medium_id: None,
412 transform_id: None,
413 calibration_id: None,
414 quantity: "haptic-mechanical-sample",
415 units: "mixed:mode-registry",
416 channels: None,
417 layout: None,
418 scalar: None,
419 model_uri: "urn:qac:mode:0x0106:v1",
420 sample_rate: Some(sample_rate),
421 valid_record_count: valid_count,
422 validity_mask_entry: mask_entry_idx,
423 source_entries: &[],
424 extension_metadata: &ext,
425 },
426 )?;
427
428 let mut entries = vec![OwnedEntry {
429 mode: MODE_HAPTIC_SAMPLE,
430 flags: 3,
431 stream_id: 1,
432 frame_id: 2,
433 start_tick: 0,
434 time_count: 1,
435 spatial_key: 0,
436 record_count: records.len() as u64,
437 stride: HAPTIC_RECORD_STRIDE,
438 descriptor_id: 6,
439 payload,
440 }];
441 if let Some(mask) = validity_mask {
443 entries.push(push_validity_mask_entry(&mut meta, 7, 3, 1, 0, 1, mask)?);
444 }
445 if let Some(ro) = receptor_output {
446 let mut ro_payload = vec![0u8; ro.len() * 4];
447 write_f32_slice(&mut ro_payload, 0, ro)?;
448 meta.push_descriptor(
449 9,
450 &DescriptorSpec {
451 clock_id: Some(1),
452 frame_id: None,
453 provenance_id: 3,
454 medium_id: None,
455 transform_id: None,
456 calibration_id: None,
457 quantity: "estimated-receptor-output",
458 units: "1",
459 channels: Some(1),
460 layout: Some("record-component"),
461 scalar: Some(AuxScalar::F32),
462 model_uri: "urn:qac:mode:0x0107:v1",
463 sample_rate: Some(sample_rate),
464 valid_record_count: None,
465 validity_mask_entry: None,
466 source_entries: &[],
467 extension_metadata: &[],
468 },
469 )?;
470 entries.push(OwnedEntry {
471 mode: MODE_AUX_ARRAY,
472 flags: 3,
473 stream_id: 1,
474 frame_id: 0,
475 start_tick: 0,
476 time_count: 1,
477 spatial_key: 0,
478 record_count: ro.len() as u64,
479 stride: 4,
480 descriptor_id: 9,
481 payload: ro_payload,
482 });
483 }
484 assemble_qac_v1(&meta, &entries, limits)
485}
486
487pub struct HapticDocumentView<'a> {
489 pub records: HapticChunkView<'a>,
490 pub role: HapticRole,
491 pub clock: ClockSpec,
492 pub contact_frame_rotation: QuaternionXyzw,
493 pub contact_area_m2: Option<f64>,
494 pub expiry_clock: Option<ClockSpec>,
495}
496
497pub fn read_haptic_qac<'a>(
499 bytes: &'a [u8],
500 entry_index: u32,
501 limits: ProfileLimits,
502) -> Result<HapticDocumentView<'a>, ProfileError> {
503 let env = AdmittedEnvelope::open_alloc(bytes, limits)?;
504 let (_entry, desc, payload, mask) = env.check_structured_entry(
505 entry_index,
506 MODE_HAPTIC_SAMPLE,
507 HAPTIC_RECORD_STRIDE,
508 false,
509 Some(205),
510 )?;
511 let clock_id = field_u32(field_req(desc, 1)?)?;
512 let clock = env.read_clock(clock_id)?;
513 let s207 = env
514 .extension_block(desc, 207)?
515 .ok_or(ProfileError::MissingRequiredSchema(207))?;
516 let role_str = field_str(field_req(s207, 1)?)?;
517 let cframe_id = field_u32(field_req(s207, 2)?)?;
518 let contact_frame_rotation = env.read_frame_quaternion(cframe_id)?;
519 let contact_area_m2 = field_opt(s207, 3).map(field_f64).transpose()?;
520 let receptor_uri = field_opt(s207, 4)
521 .map(|f| Ok::<String, ProfileError>(field_str(f)?.to_owned()))
522 .transpose()?;
523
524 let mut expiry_clock = None;
525 let role = match role_str {
526 "measurement" => HapticRole::Measurement {
527 receptor_model_uri: receptor_uri,
528 },
529 "receptor-model" => {
530 let uri = receptor_uri.ok_or(ProfileError::MissingRequiredSchema(207))?;
531 HapticRole::ReceptorModel {
532 receptor_model_uri: uri,
533 }
534 }
535 "actuator-command" => {
536 let dev_digest = field_bytes32(field_req(s207, 5)?)?;
537 let auth_digest = field_bytes32(field_req(s207, 6)?)?;
538 let exp_id = field_u32(field_req(s207, 7)?)?;
539 let (command_expiry, exp_clk) = env.read_instant(exp_id)?;
540 expiry_clock = Some(exp_clk);
541 HapticRole::ActuatorCommand {
542 device_profile_digest: dev_digest,
543 authority_receipt_digest: auth_digest,
544 command_expiry,
545 receptor_model_uri: receptor_uri,
546 }
547 }
548 _ => return Err(ProfileError::InvalidMetadata("unknown Schema 207 haptic role")),
549 };
550
551 let chunk = HapticChunkView {
552 bytes: payload,
553 mask,
554 };
555 let expected_ref = s207.id();
556 for i in 0..chunk.len() {
557 let rec = chunk.get(i)?;
558 if rec.contact_ref != expected_ref {
559 return Err(ProfileError::InvalidMetadata("HapticRecord.contact_ref disagrees with Schema 207 ID"));
560 }
561 }
562
563 Ok(HapticDocumentView {
564 records: chunk,
565 role,
566 clock,
567 contact_frame_rotation,
568 contact_area_m2,
569 expiry_clock,
570 })
571}
572
573pub fn evaluate_actuator_command_at(
579 doc: &HapticDocumentView<'_>,
580 now: &InstantSpec,
581 now_clock: &ClockSpec,
582 verify_authority: &dyn Fn(&[u8; 32], &[u8; 32]) -> bool,
583) -> Result<(), ProfileError> {
584 let (dev_digest, auth_digest, expiry) = match &doc.role {
585 HapticRole::ActuatorCommand {
586 device_profile_digest,
587 authority_receipt_digest,
588 command_expiry,
589 ..
590 } => (device_profile_digest, authority_receipt_digest, command_expiry),
591 HapticRole::Measurement { .. } | HapticRole::ReceptorModel { .. } => {
592 return Err(ProfileError::ActuationAuthority(
593 "stored haptic role is not actuator-command",
594 ));
595 }
596 };
597 let exp_clk = doc
598 .expiry_clock
599 .as_ref()
600 .ok_or(ProfileError::ActuationAuthority("missing expiry clock"))?;
601 if !verify_authority(dev_digest, auth_digest) {
602 return Err(ProfileError::ActuationAuthority("authority receipt verification failed"));
603 }
604 match now.cmp_exact(now_clock, expiry, exp_clk)? {
605 std::cmp::Ordering::Less => Ok(()),
606 std::cmp::Ordering::Equal | std::cmp::Ordering::Greater => Err(
607 ProfileError::ActuationAuthority("actuator command expired (now >= command_expiry)"),
608 ),
609 }
610}