fpm_rs/experiment/
led_array.rs

1use serde::{Deserialize, Serialize};
2
3use crate::error::{Error, Result};
4
5use super::{Optics, ResolvedSources};
6
7/// Rigid pose mapping array-local coordinates into sample coordinates.
8///
9/// Rotations are active, right-handed, extrinsic rotations about the fixed
10/// sample `x`, `y`, then `z` axes. A column vector is transformed with
11/// `Rz(rz) * Ry(ry) * Rx(rx)` before translation is added.
12#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
13#[serde(deny_unknown_fields)]
14pub struct ArrayPose {
15    /// Translation `(x, y, z)` in metres.
16    pub translation_m: [f64; 3],
17    /// Extrinsic fixed-axis `(rx, ry, rz)` rotation in radians.
18    pub rotation_rad: [f64; 3],
19    rotation_convention: ArrayRotationConvention,
20}
21
22#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
23enum ArrayRotationConvention {
24    #[serde(rename = "active_extrinsic_xyz")]
25    ActiveExtrinsicXyz,
26}
27
28impl ArrayPose {
29    /// Returns the identity rigid transform.
30    pub const fn identity() -> Self {
31        Self {
32            translation_m: [0.0; 3],
33            rotation_rad: [0.0; 3],
34            rotation_convention: ArrayRotationConvention::ActiveExtrinsicXyz,
35        }
36    }
37
38    /// Creates a pure translation in metres.
39    pub const fn from_translation(translation_m: [f64; 3]) -> Self {
40        Self {
41            translation_m,
42            rotation_rad: [0.0; 3],
43            rotation_convention: ArrayRotationConvention::ActiveExtrinsicXyz,
44        }
45    }
46
47    /// Creates a translation and active extrinsic XYZ rotation in radians.
48    pub const fn from_translation_and_extrinsic_xyz_radians(
49        translation_m: [f64; 3],
50        rotation_rad: [f64; 3],
51    ) -> Self {
52        Self {
53            translation_m,
54            rotation_rad,
55            rotation_convention: ArrayRotationConvention::ActiveExtrinsicXyz,
56        }
57    }
58
59    /// Creates a translation and active extrinsic XYZ rotation in degrees.
60    pub fn from_translation_and_extrinsic_xyz_degrees(
61        translation_m: [f64; 3],
62        rotation_deg: [f64; 3],
63    ) -> Self {
64        Self {
65            translation_m,
66            rotation_rad: rotation_deg.map(f64::to_radians),
67            rotation_convention: ArrayRotationConvention::ActiveExtrinsicXyz,
68        }
69    }
70
71    /// Validates finite translation and rotation components.
72    pub fn validate(&self) -> Result<()> {
73        if self
74            .translation_m
75            .iter()
76            .chain(&self.rotation_rad)
77            .any(|value| !value.is_finite())
78        {
79            return Err(Error::InvalidParameter {
80                name: "pose",
81                reason: "translation and rotation components must be finite".into(),
82            });
83        }
84        Ok(())
85    }
86
87    pub(crate) fn transform_point(&self, point: [f64; 3]) -> [f64; 3] {
88        let [rx, ry, rz] = self.rotation_rad;
89        let (sin_x, cos_x) = rx.sin_cos();
90        let after_x = [
91            point[0],
92            cos_x * point[1] - sin_x * point[2],
93            sin_x * point[1] + cos_x * point[2],
94        ];
95        let (sin_y, cos_y) = ry.sin_cos();
96        let after_y = [
97            cos_y * after_x[0] + sin_y * after_x[2],
98            after_x[1],
99            -sin_y * after_x[0] + cos_y * after_x[2],
100        ];
101        let (sin_z, cos_z) = rz.sin_cos();
102        let rotated = [
103            cos_z * after_y[0] - sin_z * after_y[1],
104            sin_z * after_y[0] + cos_z * after_y[1],
105            after_y[2],
106        ];
107        [
108            rotated[0] + self.translation_m[0],
109            rotated[1] + self.translation_m[1],
110            rotated[2] + self.translation_m[2],
111        ]
112    }
113}
114
115impl Default for ArrayPose {
116    fn default() -> Self {
117        Self::identity()
118    }
119}
120
121/// Regular planar LED grid with row-major physical source indexing.
122///
123/// `shape` is `(rows, columns)`, `pitch_m` is `(pitch_x, pitch_y)`, and
124/// `reference_index` is the fractional `(column, row)` lattice coordinate at
125/// the pose origin. Source index is `row * columns + column`. Per-source local
126/// Cartesian corrections are applied before the global [`ArrayPose`].
127#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
128#[serde(deny_unknown_fields)]
129pub struct PlanarLedArray {
130    shape: (usize, usize),
131    pitch_m: (f64, f64),
132    reference_index: (f64, f64),
133    pose: ArrayPose,
134    position_offsets_m: Vec<[f64; 3]>,
135}
136
137impl PlanarLedArray {
138    /// Creates a planar array from its canonical physical fields.
139    pub fn new(
140        shape: (usize, usize),
141        pitch_m: (f64, f64),
142        reference_index: (f64, f64),
143        pose: ArrayPose,
144    ) -> Self {
145        Self {
146            shape,
147            pitch_m,
148            reference_index,
149            pose,
150            position_offsets_m: Vec::new(),
151        }
152    }
153
154    /// Sets array-local per-source Cartesian corrections in metres.
155    ///
156    /// An empty vector means all-zero corrections; otherwise its length must
157    /// equal [`Self::source_count`].
158    pub fn with_position_offsets_m(mut self, offsets: Vec<[f64; 3]>) -> Self {
159        self.position_offsets_m = offsets;
160        self
161    }
162
163    /// Replaces the physical `(pitch_x, pitch_y)` lattice spacing in metres.
164    pub fn with_pitch_m(mut self, pitch_m: (f64, f64)) -> Self {
165        self.pitch_m = pitch_m;
166        self
167    }
168
169    /// Replaces the fractional `(column, row)` lattice coordinate at the pose origin.
170    pub fn with_reference_index(mut self, reference_index: (f64, f64)) -> Self {
171        self.reference_index = reference_index;
172        self
173    }
174
175    /// Replaces the active-extrinsic-XYZ rigid pose of the array.
176    pub fn with_pose(mut self, pose: ArrayPose) -> Self {
177        self.pose = pose;
178        self
179    }
180
181    /// Returns array shape as `(rows, columns)`.
182    pub const fn shape(&self) -> (usize, usize) {
183        self.shape
184    }
185
186    /// Returns pitch as `(pitch_x, pitch_y)` in metres.
187    pub const fn pitch_m(&self) -> (f64, f64) {
188        self.pitch_m
189    }
190
191    /// Returns the fractional reference lattice coordinate `(column, row)`.
192    pub const fn reference_index(&self) -> (f64, f64) {
193        self.reference_index
194    }
195
196    /// Borrows the array-local to sample-coordinate rigid pose.
197    pub const fn pose(&self) -> &ArrayPose {
198        &self.pose
199    }
200
201    /// Borrows canonical local position offsets in metres.
202    pub fn position_offsets_m(&self) -> &[[f64; 3]] {
203        &self.position_offsets_m
204    }
205
206    /// Returns `rows * columns`, or zero if an impossible platform overflow occurs.
207    pub fn source_count(&self) -> usize {
208        self.shape.0.checked_mul(self.shape.1).unwrap_or(0)
209    }
210
211    /// Converts `(row, column)` to its row-major source index.
212    pub fn source_index(&self, row: usize, column: usize) -> Result<usize> {
213        if row >= self.shape.0 || column >= self.shape.1 {
214            return Err(Error::InvalidParameter {
215                name: "source_index",
216                reason: format!("({row}, {column}) lies outside shape {:?}", self.shape),
217            });
218        }
219        row.checked_mul(self.shape.1)
220            .and_then(|value| value.checked_add(column))
221            .ok_or_else(|| Error::InvalidParameter {
222                name: "shape",
223                reason: "source index overflows".into(),
224            })
225    }
226
227    /// Converts a row-major source index to `(row, column)`.
228    pub fn source_row_column(&self, index: usize) -> Result<(usize, usize)> {
229        let count = self.validated_source_count()?;
230        if index >= count {
231            return Err(Error::InvalidParameter {
232                name: "source_index",
233                reason: format!("index {index} must be less than {count}"),
234            });
235        }
236        Ok((index / self.shape.1, index % self.shape.1))
237    }
238
239    /// Validates dimensions, pitches, reference coordinate, pose, and offsets.
240    pub fn validate(&self) -> Result<()> {
241        let count = self.validated_source_count()?;
242        if [self.pitch_m.0, self.pitch_m.1]
243            .iter()
244            .any(|value| !value.is_finite() || *value <= 0.0)
245        {
246            return Err(Error::InvalidParameter {
247                name: "pitch_m",
248                reason: "pitch_x and pitch_y must be finite and positive".into(),
249            });
250        }
251        if !self.reference_index.0.is_finite() || !self.reference_index.1.is_finite() {
252            return Err(Error::InvalidParameter {
253                name: "reference_index",
254                reason: "column and row coordinates must be finite".into(),
255            });
256        }
257        self.pose.validate()?;
258        if (!self.position_offsets_m.is_empty() && self.position_offsets_m.len() != count)
259            || self
260                .position_offsets_m
261                .iter()
262                .flatten()
263                .any(|value| !value.is_finite())
264        {
265            return Err(Error::InvalidParameter {
266                name: "position_offsets_m",
267                reason: format!("must be empty or contain {count} finite XYZ offsets"),
268            });
269        }
270        Ok(())
271    }
272
273    fn validated_source_count(&self) -> Result<usize> {
274        let count =
275            self.shape
276                .0
277                .checked_mul(self.shape.1)
278                .ok_or_else(|| Error::InvalidParameter {
279                    name: "shape",
280                    reason: "source count overflows".into(),
281                })?;
282        if count == 0 {
283            return Err(Error::InvalidParameter {
284                name: "shape",
285                reason: "rows and columns must be non-zero".into(),
286            });
287        }
288        Ok(count)
289    }
290
291    /// Resolves physical positions, directions, and transverse propagation vectors.
292    pub fn resolve(&self, optics: &Optics) -> Result<ResolvedSources> {
293        self.validate()?;
294        let count = self.validated_source_count()?;
295        let mut positions = Vec::with_capacity(count);
296        for row in 0..self.shape.0 {
297            for column in 0..self.shape.1 {
298                let index = row * self.shape.1 + column;
299                let offset = self
300                    .position_offsets_m
301                    .get(index)
302                    .copied()
303                    .unwrap_or([0.0; 3]);
304                let local = [
305                    (column as f64 - self.reference_index.0) * self.pitch_m.0 + offset[0],
306                    (row as f64 - self.reference_index.1) * self.pitch_m.1 + offset[1],
307                    offset[2],
308                ];
309                positions.push(self.pose.transform_point(local));
310            }
311        }
312        ResolvedSources::from_positions(positions, optics)
313    }
314}
fpm_rs/experiment/led_array.rs

fpm_rs/experiment/
led_array.rs
