rs_measures/inner/
affine_map_3d.rs

1#[macro_export]
2macro_rules! inner_define_affine_map_3d {
3    {} => {
4        pub struct AffineMap3d<Unit: MeasurementUnit, Number: ArithmeticOps = f64> {
5            c: [[Number; 4]; 3],
6            phantom: std::marker::PhantomData<Unit>,
7        }
8
9        impl<Unit: MeasurementUnit, Number: ArithmeticOps> AffineMap3d<Unit, Number>
10        where
11            Unit::Property: VectorProperty,
12        {
13            pub const fn new(coefficients: [[Number; 4]; 3]) -> Self {
14                Self {
15                    c: coefficients,
16                    phantom: PhantomData,
17                }
18            }
19
20            // Unit conversion.
21            pub fn convert<DestUnit: MeasurementUnit<Property = Unit::Property>>(
22                &self,
23            ) -> AffineMap3d<DestUnit, Number> {
24                let factor = Number::from_f64(Unit::RATIO / DestUnit::RATIO);
25                AffineMap3d::<DestUnit, Number>::new([
26                    [
27                        self.c[0][0],
28                        self.c[0][1],
29                        self.c[0][2],
30                        self.c[0][3] * factor,
31                    ],
32                    [
33                        self.c[1][0],
34                        self.c[1][1],
35                        self.c[1][2],
36                        self.c[1][3] * factor,
37                    ],
38                    [
39                        self.c[2][0],
40                        self.c[2][1],
41                        self.c[2][2],
42                        self.c[2][3] * factor,
43                    ],
44                ])
45            }
46
47            // Translation
48
49            pub fn translation(v: Measure3d<Unit, Number>) -> Self {
50                Self::new([
51                    [Number::ONE, Number::ZERO, Number::ZERO, v.x],
52                    [Number::ZERO, Number::ONE, Number::ZERO, v.y],
53                    [Number::ZERO, Number::ZERO, Number::ONE, v.z],
54                ])
55            }
56
57            // Rotations
58
59            // Rotation by an angle measure around a unit vector
60            // applied to a point.
61            // Precondition: unit_vector.squared_norm().value == 1
62            pub fn rotation<AngleUnit: AngleMeasurementUnit<Property = Angle>, AxisUnit: MeasurementUnit>(
63                fixed_point: MeasurePoint3d<Unit, Number>,
64                unit_vector: Measure3d<AxisUnit, Number>,
65                angle: Measure<AngleUnit, Number>,
66            ) -> Self
67            where
68                AxisUnit::Property: VectorProperty,
69            {
70                let fpx = fixed_point.x;
71                let fpy = fixed_point.y;
72                let fpz = fixed_point.z;
73                let ux = unit_vector.x;
74                let uy = unit_vector.y;
75                let uz = unit_vector.z;
76                let a = angle.convert::<Radian>().value;
77                let (sin_a, cos_a) = a.sin_cos();
78                let one_minus_cos_a = Number::ONE - cos_a;
79                let c00 = cos_a + ux * ux * one_minus_cos_a;
80                let c01 = ux * uy * one_minus_cos_a - uz * sin_a;
81                let c02 = ux * uz * one_minus_cos_a + uy * sin_a;
82                let c10 = uy * ux * one_minus_cos_a + uz * sin_a;
83                let c11 = cos_a + uy * uy * one_minus_cos_a;
84                let c12 = uy * uz * one_minus_cos_a - ux * sin_a;
85                let c20 = uz * ux * one_minus_cos_a - uy * sin_a;
86                let c21 = uz * uy * one_minus_cos_a + ux * sin_a;
87                let c22 = cos_a + uz * uz * one_minus_cos_a;
88                Self::new([
89                    [c00, c01, c02, fpx - fpx * c00 - fpy * c01 - fpz * c02],
90                    [c10, c11, c12, fpy - fpx * c10 - fpy * c11 - fpz * c12],
91                    [c20, c21, c22, fpz - fpx * c20 - fpy * c21 - fpz * c22],
92                ])
93            }
94
95            // Projections
96
97            // Projection onto a line identified by a unit vector
98            // applied to a point.
99            // Precondition: unit_vector.squared_norm().value == 1
100            pub fn projection_onto_line<AxisUnit: MeasurementUnit>(
101                fixed_point: MeasurePoint3d<Unit, Number>,
102                unit_vector: Measure3d<AxisUnit, Number>,
103            ) -> Self {
104                let fpx = fixed_point.x;
105                let fpy = fixed_point.y;
106                let fpz = fixed_point.z;
107                let ux = unit_vector.x;
108                let uy = unit_vector.y;
109                let uz = unit_vector.z;
110                Self::new([
111                    [
112                        ux * ux,
113                        uy * ux,
114                        uz * ux,
115                        fpx - fpx * ux * ux - fpy * uy * ux - fpz * uz * ux,
116                    ],
117                    [
118                        ux * uy,
119                        uy * uy,
120                        uz * uy,
121                        fpy - fpx * ux * uy - fpy * uy * uy - fpz * uz * uy,
122                    ],
123                    [
124                        ux * uz,
125                        uy * uz,
126                        uz * uz,
127                        fpz - fpx * ux * uz - fpy * uy * uz - fpz * uz * uz,
128                    ],
129                ])
130            }
131
132            // Projection onto a plane whose normal is identified by a unit vector.
133            // applied to a point.
134            // Precondition: unit_vector.squared_norm().value == 1
135            pub fn projection_onto_plane<AxisUnit: MeasurementUnit>(
136                fixed_point: MeasurePoint3d<Unit, Number>,
137                unit_vector: Measure3d<AxisUnit, Number>,
138            ) -> Self {
139                let fpx = fixed_point.x;
140                let fpy = fixed_point.y;
141                let fpz = fixed_point.z;
142                let ux = unit_vector.x;
143                let uy = unit_vector.y;
144                let uz = unit_vector.z;
145                let c00 = Number::ONE - unit_vector.x * unit_vector.x;
146                let c01 = -unit_vector.y * unit_vector.x;
147                let c02 = -unit_vector.z * unit_vector.x;
148                let c10 = -unit_vector.x * unit_vector.y;
149                let c11 = Number::ONE - unit_vector.y * unit_vector.y;
150                let c12 = -unit_vector.z * unit_vector.y;
151                let c20 = -unit_vector.x * unit_vector.z;
152                let c21 = -unit_vector.y * unit_vector.z;
153                let c22 = Number::ONE - unit_vector.z * unit_vector.z;
154                Self::new([
155                    [c00, c01, c02, fpx - fpx * c00 - fpy * c01 - fpz * c02],
156                    [c10, c11, c12, fpy - fpx * c10 - fpy * c11 - fpz * c12],
157                    [c20, c21, c22, fpz - fpx * c20 - fpy * c21 - fpz * c22],
158                ])
159            }
160
161            // Reflections
162
163            // Reflection over a line identified by a unit vector.
164            // applied to a point.
165            // Precondition: unit_vector.squared_norm().value == 1
166            pub fn reflection_over_line<AxisUnit: MeasurementUnit>(
167                fixed_point: MeasurePoint3d<Unit, Number>,
168                unit_vector: Measure3d<AxisUnit, Number>,
169            ) -> Self {
170                let two = Number::ONE + Number::ONE;
171                let fpx = fixed_point.x;
172                let fpy = fixed_point.y;
173                let fpz = fixed_point.z;
174                let ux = unit_vector.x;
175                let uy = unit_vector.y;
176                let uz = unit_vector.z;
177                let c00 = two * ux * ux - Number::ONE;
178                let c01 = two * uy * ux;
179                let c02 = two * uz * ux;
180                let c10 = two * ux * uy;
181                let c11 = two * uy * uy - Number::ONE;
182                let c12 = two * uz * uy;
183                let c20 = two * ux * uz;
184                let c21 = two * uy * uz;
185                let c22 = two * uz * uz - Number::ONE;
186                Self::new([
187                    [c00, c01, c02, fpx - fpx * c00 - fpy * c01 - fpz * c02],
188                    [c10, c11, c12, fpy - fpx * c10 - fpy * c11 - fpz * c12],
189                    [c20, c21, c22, fpz - fpx * c20 - fpy * c21 - fpz * c22],
190                ])
191            }
192
193            // Reflection over a plane whose normal is identified by a unit vector.
194            // applied to a point.
195            // Precondition: unit_vector.squared_norm().value == 1
196            pub fn reflection_over_plane<AxisUnit: MeasurementUnit>(
197                fixed_point: MeasurePoint3d<Unit, Number>,
198                unit_vector: Measure3d<AxisUnit, Number>,
199            ) -> Self {
200                let minus_two = -(Number::ONE + Number::ONE);
201                let fpx = fixed_point.x;
202                let fpy = fixed_point.y;
203                let fpz = fixed_point.z;
204                let ux = unit_vector.x;
205                let uy = unit_vector.y;
206                let uz = unit_vector.z;
207                let c00 = minus_two * unit_vector.x * unit_vector.x + Number::ONE;
208                let c01 = minus_two * unit_vector.y * unit_vector.x;
209                let c02 = minus_two * unit_vector.z * unit_vector.x;
210                let c10 = minus_two * unit_vector.x * unit_vector.y;
211                let c11 = minus_two * unit_vector.y * unit_vector.y + Number::ONE;
212                let c12 = minus_two * unit_vector.z * unit_vector.y;
213                let c20 = minus_two * unit_vector.x * unit_vector.z;
214                let c21 = minus_two * unit_vector.y * unit_vector.z;
215                let c22 = minus_two * unit_vector.z * unit_vector.z + Number::ONE;
216                Self::new([
217                    [c00, c01, c02, fpx - fpx * c00 - fpy * c01 - fpz * c02],
218                    [c10, c11, c12, fpy - fpx * c10 - fpy * c11 - fpz * c12],
219                    [c20, c21, c22, fpz - fpx * c20 - fpy * c21 - fpz * c22],
220                ])
221            }
222
223            // Scaling by three factors from a point.
224
225            pub fn scaling(
226                fixed_point: MeasurePoint3d<Unit, Number>,
227                kx: Number,
228                ky: Number,
229                kz: Number,
230            ) -> Self {
231                Self::new([
232                    [
233                        kx,
234                        Number::ZERO,
235                        Number::ZERO,
236                        fixed_point.x * (Number::ONE - kx),
237                    ],
238                    [
239                        Number::ZERO,
240                        ky,
241                        Number::ZERO,
242                        fixed_point.y * (Number::ONE - ky),
243                    ],
244                    [
245                        Number::ZERO,
246                        Number::ZERO,
247                        kz,
248                        fixed_point.z * (Number::ONE - kz),
249                    ],
250                ])
251            }
252
253            // Inversion
254
255            pub fn inverted(&self) -> Self {
256                let inv_determinant = Number::ONE
257                    / (self.c[0][0] * (self.c[1][1] * self.c[2][2] - self.c[1][2] * self.c[2][1])
258                        - self.c[0][1] * (self.c[1][0] * self.c[2][2] - self.c[1][2] * self.c[2][0])
259                        + self.c[0][2] * (self.c[1][0] * self.c[2][1] - self.c[1][1] * self.c[2][0]));
260                let c00 = (self.c[1][1] * self.c[2][2] - self.c[1][2] * self.c[2][1]) * inv_determinant;
261                let c01 = -(self.c[0][1] * self.c[2][2] - self.c[0][2] * self.c[2][1]) * inv_determinant;
262                let c02 = (self.c[0][1] * self.c[1][2] - self.c[0][2] * self.c[1][1]) * inv_determinant;
263                let c10 = -(self.c[1][0] * self.c[2][2] - self.c[1][2] * self.c[2][0]) * inv_determinant;
264                let c11 = (self.c[0][0] * self.c[2][2] - self.c[0][2] * self.c[2][0]) * inv_determinant;
265                let c12 = -(self.c[0][0] * self.c[1][2] - self.c[0][2] * self.c[1][0]) * inv_determinant;
266                let c20 = (self.c[1][0] * self.c[2][1] - self.c[1][1] * self.c[2][0]) * inv_determinant;
267                let c21 = -(self.c[0][0] * self.c[2][1] - self.c[0][1] * self.c[2][0]) * inv_determinant;
268                let c22 = (self.c[0][0] * self.c[1][1] - self.c[0][1] * self.c[1][0]) * inv_determinant;
269                Self::new([
270                    [
271                        c00,
272                        c01,
273                        c02,
274                        -c00 * self.c[0][3] - c01 * self.c[1][3] - c02 * self.c[2][3],
275                    ],
276                    [
277                        c10,
278                        c11,
279                        c12,
280                        -c10 * self.c[0][3] - c11 * self.c[1][3] - c12 * self.c[2][3],
281                    ],
282                    [
283                        c20,
284                        c21,
285                        c22,
286                        -c20 * self.c[0][3] - c21 * self.c[1][3] - c22 * self.c[2][3],
287                    ],
288                ])
289            }
290
291            // Composition of spacial affine transformations.
292            // To apply the resulting transformation is equivalent to apply first
293            // `other` and then `self`.
294            pub fn combined_with(&self, other: &AffineMap3d<Unit, Number>) -> Self {
295                Self::new([
296                    [
297                        other.c[0][0] * self.c[0][0]
298                            + other.c[0][1] * self.c[1][0]
299                            + other.c[0][2] * self.c[2][0],
300                        other.c[0][0] * self.c[0][1]
301                            + other.c[0][1] * self.c[1][1]
302                            + other.c[0][2] * self.c[2][1],
303                        other.c[0][0] * self.c[0][2]
304                            + other.c[0][1] * self.c[1][2]
305                            + other.c[0][2] * self.c[2][2],
306                        other.c[0][0] * self.c[0][3]
307                            + other.c[0][1] * self.c[1][3]
308                            + other.c[0][2] * self.c[2][3]
309                            + other.c[0][3],
310                    ],
311                    [
312                        other.c[1][0] * self.c[0][0]
313                            + other.c[1][1] * self.c[1][0]
314                            + other.c[1][2] * self.c[2][0],
315                        other.c[1][0] * self.c[0][1]
316                            + other.c[1][1] * self.c[1][1]
317                            + other.c[1][2] * self.c[2][1],
318                        other.c[1][0] * self.c[0][2]
319                            + other.c[1][1] * self.c[1][2]
320                            + other.c[1][2] * self.c[2][2],
321                        other.c[1][0] * self.c[0][3]
322                            + other.c[1][1] * self.c[1][3]
323                            + other.c[1][2] * self.c[2][3]
324                            + other.c[1][3],
325                    ],
326                    [
327                        other.c[2][0] * self.c[0][0]
328                            + other.c[2][1] * self.c[1][0]
329                            + other.c[2][2] * self.c[2][0],
330                        other.c[2][0] * self.c[0][1]
331                            + other.c[2][1] * self.c[1][1]
332                            + other.c[2][2] * self.c[2][1],
333                        other.c[2][0] * self.c[0][2]
334                            + other.c[2][1] * self.c[1][2]
335                            + other.c[2][2] * self.c[2][2],
336                        other.c[2][0] * self.c[0][3]
337                            + other.c[2][1] * self.c[1][3]
338                            + other.c[2][2] * self.c[2][3]
339                            + other.c[2][3],
340                    ],
341                ])
342            }
343
344            pub fn apply_to(&self, m: MeasurePoint3d<Unit, Number>) -> MeasurePoint3d<Unit, Number> {
345                MeasurePoint3d::<Unit, Number>::new(
346                    self.c[0][0] * m.x + self.c[0][1] * m.y + self.c[0][2] * m.z + self.c[0][3],
347                    self.c[1][0] * m.x + self.c[1][1] * m.y + self.c[1][2] * m.z + self.c[1][3],
348                    self.c[2][0] * m.x + self.c[2][1] * m.y + self.c[2][2] * m.z + self.c[2][3],
349                )
350            }
351        }
352
353        impl<Unit, Number> Default for AffineMap3d<Unit, Number>
354        where
355            Unit: MeasurementUnit,
356            Number: ArithmeticOps,
357            Unit::Property: VectorProperty,
358        {
359            // It returns the identity transformation.
360            fn default() -> Self {
361                Self::new([
362                    [Number::ONE, Number::ZERO, Number::ZERO, Number::ZERO],
363                    [Number::ZERO, Number::ONE, Number::ZERO, Number::ZERO],
364                    [Number::ZERO, Number::ZERO, Number::ONE, Number::ZERO],
365                ])
366            }
367        }
368
369        // format!("{}", AffineMap3d)
370        impl<Unit: MeasurementUnit, Number: ArithmeticOps> fmt::Display for AffineMap3d<Unit, Number>
371        where
372            Unit::Property: VectorProperty,
373        {
374            fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
375                write!(
376                    formatter,
377                    "{}",
378                    rs_measures::matrix_utils::format_matrix::<3, 4, Number>(&self.c, Unit::SUFFIX)
379                )
380            }
381        }
382
383        // format!("{:?}", AffineMap3d)
384        impl<Unit: MeasurementUnit, Number: ArithmeticOps> fmt::Debug for AffineMap3d<Unit, Number>
385        where
386            Unit::Property: VectorProperty,
387        {
388            fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
389                write!(
390                    formatter,
391                    "{}",
392                    rs_measures::matrix_utils::format_matrix::<3, 4, Number>(&self.c, Unit::SUFFIX)
393                )
394            }
395        }
396    };
397}
rs_measures/inner/affine_map_3d.rs

rs_measures/inner/
affine_map_3d.rs
