1use super::types::{
5 validate_identifier_str, ProfileError, BASE_FRAME_CONVENTION, DIRECTION_NORM_TOLERANCE,
6 MAX_CLOCK_DEN, MAX_CLOCK_NUM, QUATERNION_NORM_TOLERANCE,
7};
8
9#[derive(Debug, Clone, Copy, PartialEq)]
18pub struct QuaternionXyzw {
19 pub x: f64,
20 pub y: f64,
21 pub z: f64,
22 pub w: f64,
23}
24
25impl QuaternionXyzw {
26 pub const IDENTITY: Self = Self {
27 x: 0.0,
28 y: 0.0,
29 z: 0.0,
30 w: 1.0,
31 };
32
33 pub fn from_xyzw(xyzw: [f64; 4]) -> Result<Self, ProfileError> {
34 for v in xyzw {
35 if !v.is_finite() {
36 return Err(ProfileError::InvalidQuaternion);
37 }
38 }
39 let [x, y, z, w] = xyzw;
40 let sq = ((x * x + y * y) + z * z) + w * w;
41 if (sq - 1.0).abs() > QUATERNION_NORM_TOLERANCE {
42 return Err(ProfileError::InvalidQuaternion);
43 }
44 Ok(Self { x, y, z, w })
45 }
46
47 pub fn from_axis_angle(axis: [f64; 3], angle_rad: f64) -> Result<Self, ProfileError> {
48 if !angle_rad.is_finite() || axis.iter().any(|v| !v.is_finite()) {
49 return Err(ProfileError::InvalidQuaternion);
50 }
51 let norm_sq = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2];
52 if norm_sq <= 0.0 {
53 return Err(ProfileError::InvalidQuaternion);
54 }
55 let inv = 1.0 / norm_sq.sqrt();
56 let half = 0.5 * angle_rad;
57 let s = half.sin();
58 Self::from_xyzw([axis[0] * inv * s, axis[1] * inv * s, axis[2] * inv * s, half.cos()])
59 }
60
61 pub fn normalized_for_derived_use(self) -> [f64; 4] {
63 let norm = (((self.x * self.x + self.y * self.y) + self.z * self.z) + self.w * self.w).sqrt();
64 [self.x / norm, self.y / norm, self.z / norm, self.w / norm]
65 }
66
67 pub fn inverse(self) -> Self {
68 Self {
69 x: -self.x,
70 y: -self.y,
71 z: -self.z,
72 w: self.w,
73 }
74 }
75
76 pub fn rotation_matrix(self) -> [[f64; 3]; 3] {
78 let [x, y, z, w] = self.normalized_for_derived_use();
79 let xx = x * x;
80 let yy = y * y;
81 let zz = z * z;
82 let xy = x * y;
83 let xz = x * z;
84 let yz = y * z;
85 let wx = w * x;
86 let wy = w * y;
87 let wz = w * z;
88 [
89 [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
90 [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
91 [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)],
92 ]
93 }
94
95 pub fn rotate_vec3(self, v: [f64; 3]) -> [f64; 3] {
96 let r = self.rotation_matrix();
97 [
98 r[0][0] * v[0] + r[0][1] * v[1] + r[0][2] * v[2],
99 r[1][0] * v[0] + r[1][1] * v[1] + r[1][2] * v[2],
100 r[2][0] * v[0] + r[2][1] * v[1] + r[2][2] * v[2],
101 ]
102 }
103
104 pub fn rotate_symmetric_tensor6(self, s: [f64; 6]) -> [f64; 6] {
106 let r = self.rotation_matrix();
107 let mat = [
108 [s[0], s[3], s[5]],
109 [s[3], s[1], s[4]],
110 [s[5], s[4], s[2]],
111 ];
112 let mut rs = [[0.0f64; 3]; 3];
113 for i in 0..3 {
114 for j in 0..3 {
115 rs[i][j] = r[i][0] * mat[0][j] + r[i][1] * mat[1][j] + r[i][2] * mat[2][j];
116 }
117 }
118 let mut out = [[0.0f64; 3]; 3];
119 for i in 0..3 {
120 for j in 0..3 {
121 out[i][j] = rs[i][0] * r[j][0] + rs[i][1] * r[j][1] + rs[i][2] * r[j][2];
122 }
123 }
124 [
125 out[0][0],
126 out[1][1],
127 out[2][2],
128 0.5 * (out[0][1] + out[1][0]),
129 0.5 * (out[1][2] + out[2][1]),
130 0.5 * (out[0][2] + out[2][0]),
131 ]
132 }
133}
134
135#[derive(Debug, Clone, Copy, PartialEq)]
137pub struct UnitDirection3(pub [f32; 3]);
138
139impl UnitDirection3 {
140 pub fn new(v: [f32; 3]) -> Result<Self, ProfileError> {
141 for c in v {
142 if !c.is_finite() {
143 return Err(ProfileError::NonFiniteFloat);
144 }
145 }
146 let x = f64::from(v[0]);
147 let y = f64::from(v[1]);
148 let z = f64::from(v[2]);
149 let sq = (x * x + y * y) + z * z;
150 if (sq - 1.0).abs() > DIRECTION_NORM_TOLERANCE {
151 return Err(ProfileError::InvalidUnitVector);
152 }
153 Ok(Self(v))
154 }
155}
156
157#[inline]
162pub const fn gcd_u64(mut a: u64, mut b: u64) -> u64 {
163 while b != 0 {
164 let r = a % b;
165 a = b;
166 b = r;
167 }
168 a
169}
170
171#[derive(Debug, Clone, Copy, PartialEq, Eq)]
172pub struct RationalRate {
173 pub num: u64,
174 pub den: u64,
175}
176
177impl RationalRate {
178 pub fn new(num: u64, den: u64) -> Result<Self, ProfileError> {
179 if num == 0 || den == 0 {
180 return Err(ProfileError::InvalidTimeSpec);
181 }
182 let g = gcd_u64(num, den);
183 let rn = num / g;
184 let rd = den / g;
185 if rn > MAX_CLOCK_NUM || rd > MAX_CLOCK_DEN {
186 return Err(ProfileError::InvalidTimeSpec);
187 }
188 Ok(Self { num: rn, den: rd })
189 }
190}
191
192#[derive(Debug, Clone, Copy, PartialEq, Eq)]
193pub struct ClockSpec {
194 pub scale: u32,
195 pub epoch_seconds: i64,
196 pub epoch_nanoseconds: u32,
197 pub ticks_per_second: RationalRate,
198}
199
200impl ClockSpec {
201 pub fn validate(&self) -> Result<(), ProfileError> {
202 if self.scale > 2 || self.epoch_nanoseconds >= 1_000_000_000 {
203 return Err(ProfileError::InvalidTimeSpec);
204 }
205 if self.ticks_per_second.num == 0
206 || self.ticks_per_second.den == 0
207 || gcd_u64(self.ticks_per_second.num, self.ticks_per_second.den) != 1
208 || self.ticks_per_second.num > MAX_CLOCK_NUM
209 || self.ticks_per_second.den > MAX_CLOCK_DEN
210 {
211 return Err(ProfileError::InvalidTimeSpec);
212 }
213 Ok(())
214 }
215}
216
217#[derive(Debug, Clone, Copy, PartialEq, Eq)]
219pub struct InstantSpec {
220 pub clock_block_id: u32,
221 pub tick: i64,
222 pub subtick_num: u64,
223 pub subtick_den: u64,
224}
225
226impl InstantSpec {
227 pub fn new(
228 clock_block_id: u32,
229 tick: i64,
230 subtick_num: u64,
231 subtick_den: u64,
232 ) -> Result<Self, ProfileError> {
233 if clock_block_id == 0
234 || subtick_den == 0
235 || subtick_num >= subtick_den
236 || subtick_den > MAX_CLOCK_DEN
237 {
238 return Err(ProfileError::InvalidTimeSpec);
239 }
240 if (subtick_num == 0 && subtick_den != 1)
241 || (subtick_num != 0 && gcd_u64(subtick_num, subtick_den) != 1)
242 {
243 return Err(ProfileError::InvalidTimeSpec);
244 }
245 Ok(Self {
246 clock_block_id,
247 tick,
248 subtick_num,
249 subtick_den,
250 })
251 }
252
253 pub fn as_exact_tai_or_epoch_seconds(
255 &self,
256 clock: &ClockSpec,
257 ) -> Result<(i128, i128), ProfileError> {
258 clock.validate()?;
259 let sub_den = i128::from(self.subtick_den);
260 let t_num = i128::from(clock.ticks_per_second.num);
261 let t_den = i128::from(clock.ticks_per_second.den);
262 let total_ticks_num = i128::from(self.tick)
263 .checked_mul(sub_den)
264 .and_then(|v| v.checked_add(i128::from(self.subtick_num)))
265 .ok_or(ProfileError::Overflow)?;
266 let rel_num = total_ticks_num.checked_mul(t_den).ok_or(ProfileError::Overflow)?;
267 let rel_den = sub_den.checked_mul(t_num).ok_or(ProfileError::Overflow)?;
268 let epoch_ns = i128::from(clock.epoch_seconds)
269 .checked_mul(1_000_000_000)
270 .and_then(|v| v.checked_add(i128::from(clock.epoch_nanoseconds)))
271 .ok_or(ProfileError::Overflow)?;
272 let final_num = epoch_ns
273 .checked_mul(rel_den)
274 .and_then(|lhs| lhs.checked_add(rel_num.checked_mul(1_000_000_000)?))
275 .ok_or(ProfileError::Overflow)?;
276 let final_den = 1_000_000_000i128
277 .checked_mul(rel_den)
278 .ok_or(ProfileError::Overflow)?;
279 Ok((final_num, final_den))
280 }
281
282 pub fn cmp_exact(
284 &self,
285 self_clock: &ClockSpec,
286 other: &InstantSpec,
287 other_clock: &ClockSpec,
288 ) -> Result<std::cmp::Ordering, ProfileError> {
289 if self_clock.scale != other_clock.scale {
290 return Err(ProfileError::ClockScaleMismatch);
291 }
292 let (ln, ld) = self.as_exact_tai_or_epoch_seconds(self_clock)?;
293 let (rn, rd) = other.as_exact_tai_or_epoch_seconds(other_clock)?;
294 if let (Some(lhs), Some(rhs)) = (ln.checked_mul(rd), rn.checked_mul(ld)) {
295 return Ok(lhs.cmp(&rhs));
296 }
297 let g = gcd_i128(ld, rd);
298 let ld_r = ld / g;
299 let rd_r = rd / g;
300 let lhs = ln.checked_mul(rd_r).ok_or(ProfileError::Overflow)?;
301 let rhs = rn.checked_mul(ld_r).ok_or(ProfileError::Overflow)?;
302 Ok(lhs.cmp(&rhs))
303 }
304}
305
306#[inline]
307const fn gcd_i128(mut a: i128, mut b: i128) -> i128 {
308 a = a.abs();
309 b = b.abs();
310 while b != 0 {
311 let r = a % b;
312 a = b;
313 b = r;
314 }
315 if a == 0 { 1 } else { a }
316}
317
318#[derive(Debug, Clone, PartialEq)]
320pub struct FrameSpec<'a> {
321 pub kind: u32,
322 pub parent_block_id: Option<u32>,
323 pub origin_m: [f64; 3],
324 pub rotation: QuaternionXyzw,
325 pub grid_spacing_m: Option<[f64; 3]>,
326 pub grid_dimensions: Option<[u32; 3]>,
327 pub index_order: Option<u32>,
328 pub convention: &'a str,
329 pub crs_definition: Option<&'a [u8]>,
330}
331
332impl<'a> FrameSpec<'a> {
333 pub fn local_default() -> Self {
334 Self {
335 kind: 0,
336 parent_block_id: None,
337 origin_m: [0.0, 0.0, 0.0],
338 rotation: QuaternionXyzw::IDENTITY,
339 grid_spacing_m: None,
340 grid_dimensions: None,
341 index_order: None,
342 convention: BASE_FRAME_CONVENTION,
343 crs_definition: None,
344 }
345 }
346}
347
348#[derive(Debug, Clone)]
350pub struct ProvenanceSpec<'a> {
351 pub role: u32,
352 pub source_digests: &'a [[u8; 32]],
353 pub algorithm: Option<&'a str>,
354 pub algorithm_version: Option<&'a str>,
355 pub instrument_uri: Option<&'a str>,
356 pub authority_receipt_digest: Option<[u8; 32]>,
357 pub lineage: Option<&'a str>,
358}
359
360#[derive(Debug, Clone)]
362pub struct CalibrationSpec<'a> {
363 pub input_quantity: &'a str,
364 pub output_units: &'a str,
365 pub scale: f64,
366 pub offset: f64,
367 pub reference_pressure_pa: Option<f64>,
368 pub response_entry: Option<u32>,
369 pub uncertainty_entry: Option<u32>,
370 pub calibration_digest: Option<[u8; 32]>,
371 pub valid_from: Option<u32>,
372 pub valid_until: Option<u32>,
373}
374
375impl<'a> CalibrationSpec<'a> {
376 pub fn validate(&self) -> Result<(), ProfileError> {
377 validate_identifier_str(self.input_quantity, "calibration.input_quantity")?;
378 validate_identifier_str(self.output_units, "calibration.output_units")?;
379 if !self.scale.is_finite() || self.scale == 0.0 || !self.offset.is_finite() {
380 return Err(ProfileError::InvalidMetadata("calibration.scale_offset"));
381 }
382 if let Some(p0) = self.reference_pressure_pa {
383 if !p0.is_finite() || p0 <= 0.0 {
384 return Err(ProfileError::InvalidMetadata("calibration.reference_pressure_pa"));
385 }
386 }
387 Ok(())
388 }
389
390 pub fn is_valid_at(
392 now: &InstantSpec,
393 now_clock: &ClockSpec,
394 from: Option<(&InstantSpec, &ClockSpec)>,
395 until: Option<(&InstantSpec, &ClockSpec)>,
396 ) -> Result<bool, ProfileError> {
397 if let (Some((f_inst, f_clk)), Some((u_inst, u_clk))) = (from, until) {
398 if f_inst.cmp_exact(f_clk, u_inst, u_clk)? != std::cmp::Ordering::Less {
399 return Err(ProfileError::InvalidTimeSpec);
400 }
401 }
402 if let Some((f_inst, f_clk)) = from {
403 if now.cmp_exact(now_clock, f_inst, f_clk)? == std::cmp::Ordering::Less {
404 return Ok(false);
405 }
406 }
407 if let Some((u_inst, u_clk)) = until {
408 if now.cmp_exact(now_clock, u_inst, u_clk)? != std::cmp::Ordering::Less {
409 return Ok(false);
410 }
411 }
412 Ok(true)
413 }
414}