moxcms/
matrix.rs

1/*
2 * // Copyright (c) Radzivon Bartoshyk 2/2025. All rights reserved.
3 * //
4 * // Redistribution and use in source and binary forms, with or without modification,
5 * // are permitted provided that the following conditions are met:
6 * //
7 * // 1.  Redistributions of source code must retain the above copyright notice, this
8 * // list of conditions and the following disclaimer.
9 * //
10 * // 2.  Redistributions in binary form must reproduce the above copyright notice,
11 * // this list of conditions and the following disclaimer in the documentation
12 * // and/or other materials provided with the distribution.
13 * //
14 * // 3.  Neither the name of the copyright holder nor the names of its
15 * // contributors may be used to endorse or promote products derived from
16 * // this software without specific prior written permission.
17 * //
18 * // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 * // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 * // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 * // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 * // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 * // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 * // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 * // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 * // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 * // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 */
29use crate::math::{FusedMultiplyAdd, FusedMultiplyNegAdd};
30use crate::mlaf::{mlaf, neg_mlaf};
31use crate::reader::s15_fixed16_number_to_double;
32use num_traits::{AsPrimitive, MulAdd};
33use std::ops::{Add, Div, Mul, Neg, Shr, Sub};
34
35/// Vector math helper
36#[repr(transparent)]
37#[derive(Copy, Clone, Debug, Default)]
38pub struct Vector3<T> {
39    pub v: [T; 3],
40}
41
42/// Vector math helper
43#[repr(transparent)]
44#[derive(Copy, Clone, Debug, Default)]
45pub struct Vector4<T> {
46    pub v: [T; 4],
47}
48
49pub type Vector4f = Vector4<f32>;
50pub type Vector4d = Vector4<f64>;
51pub type Vector4i = Vector4<i32>;
52
53pub type Vector3f = Vector3<f32>;
54pub type Vector3d = Vector3<f64>;
55pub type Vector3i = Vector3<i32>;
56pub type Vector3u = Vector3<u32>;
57
58impl<T> PartialEq<Self> for Vector3<T>
59where
60    T: AsPrimitive<f32>,
61{
62    #[inline(always)]
63    fn eq(&self, other: &Self) -> bool {
64        const TOLERANCE: f32 = 0.0001f32;
65        let dx = (self.v[0].as_() - other.v[0].as_()).abs();
66        let dy = (self.v[1].as_() - other.v[1].as_()).abs();
67        let dz = (self.v[2].as_() - other.v[2].as_()).abs();
68        dx < TOLERANCE && dy < TOLERANCE && dz < TOLERANCE
69    }
70}
71
72impl<T> Vector3<T> {
73    #[inline(always)]
74    pub fn to_<Z: Copy + 'static>(self) -> Vector3<Z>
75    where
76        T: AsPrimitive<Z>,
77    {
78        Vector3 {
79            v: [self.v[0].as_(), self.v[1].as_(), self.v[2].as_()],
80        }
81    }
82}
83
84impl<T> Mul<Vector3<T>> for Vector3<T>
85where
86    T: Mul<Output = T> + Copy,
87{
88    type Output = Vector3<T>;
89
90    #[inline(always)]
91    fn mul(self, rhs: Vector3<T>) -> Self::Output {
92        Self {
93            v: [
94                self.v[0] * rhs.v[0],
95                self.v[1] * rhs.v[1],
96                self.v[2] * rhs.v[2],
97            ],
98        }
99    }
100}
101
102impl<T: Copy> Shr<i32> for Vector3<T>
103where
104    T: Shr<i32, Output = T>,
105{
106    type Output = Vector3<T>;
107    fn shr(self, rhs: i32) -> Self::Output {
108        Self {
109            v: [self.v[0] >> rhs, self.v[1] >> rhs, self.v[2] >> rhs],
110        }
111    }
112}
113
114impl<T: Copy> Shr<i32> for Vector4<T>
115where
116    T: Shr<i32, Output = T>,
117{
118    type Output = Vector4<T>;
119    fn shr(self, rhs: i32) -> Self::Output {
120        Self {
121            v: [
122                self.v[0] >> rhs,
123                self.v[1] >> rhs,
124                self.v[2] >> rhs,
125                self.v[3] >> rhs,
126            ],
127        }
128    }
129}
130
131impl<T> Mul<Vector4<T>> for Vector4<T>
132where
133    T: Mul<Output = T> + Copy,
134{
135    type Output = Vector4<T>;
136
137    #[inline(always)]
138    fn mul(self, rhs: Vector4<T>) -> Self::Output {
139        Self {
140            v: [
141                self.v[0] * rhs.v[0],
142                self.v[1] * rhs.v[1],
143                self.v[2] * rhs.v[2],
144                self.v[3] * rhs.v[3],
145            ],
146        }
147    }
148}
149
150impl<T> Mul<T> for Vector3<T>
151where
152    T: Mul<Output = T> + Copy,
153{
154    type Output = Vector3<T>;
155
156    #[inline(always)]
157    fn mul(self, rhs: T) -> Self::Output {
158        Self {
159            v: [self.v[0] * rhs, self.v[1] * rhs, self.v[2] * rhs],
160        }
161    }
162}
163
164impl Vector3<f32> {
165    #[inline(always)]
166    const fn const_mul_vector(self, v: Vector3f) -> Vector3f {
167        Vector3f {
168            v: [self.v[0] * v.v[0], self.v[1] * v.v[1], self.v[2] * v.v[2]],
169        }
170    }
171}
172
173impl Vector3d {
174    #[inline(always)]
175    const fn const_mul_vector(self, v: Vector3d) -> Vector3d {
176        Vector3d {
177            v: [self.v[0] * v.v[0], self.v[1] * v.v[1], self.v[2] * v.v[2]],
178        }
179    }
180}
181
182impl<T: 'static> Vector3<T> {
183    pub fn cast<V: Copy + 'static>(&self) -> Vector3<V>
184    where
185        T: AsPrimitive<V>,
186    {
187        Vector3::<V> {
188            v: [self.v[0].as_(), self.v[1].as_(), self.v[2].as_()],
189        }
190    }
191}
192
193impl<T> Mul<T> for Vector4<T>
194where
195    T: Mul<Output = T> + Copy,
196{
197    type Output = Vector4<T>;
198
199    #[inline(always)]
200    fn mul(self, rhs: T) -> Self::Output {
201        Self {
202            v: [
203                self.v[0] * rhs,
204                self.v[1] * rhs,
205                self.v[2] * rhs,
206                self.v[3] * rhs,
207            ],
208        }
209    }
210}
211
212impl<T: Copy + Mul<T, Output = T> + Add<T, Output = T> + MulAdd<T, Output = T>>
213    FusedMultiplyAdd<Vector3<T>> for Vector3<T>
214{
215    #[inline(always)]
216    fn mla(&self, b: Vector3<T>, c: Vector3<T>) -> Vector3<T> {
217        let x0 = mlaf(self.v[0], b.v[0], c.v[0]);
218        let x1 = mlaf(self.v[1], b.v[1], c.v[1]);
219        let x2 = mlaf(self.v[2], b.v[2], c.v[2]);
220        Vector3 { v: [x0, x1, x2] }
221    }
222}
223
224impl<T: Copy + Mul<T, Output = T> + Add<T, Output = T> + MulAdd<T, Output = T> + Neg<Output = T>>
225    FusedMultiplyNegAdd<Vector3<T>> for Vector3<T>
226{
227    #[inline(always)]
228    fn neg_mla(&self, b: Vector3<T>, c: Vector3<T>) -> Vector3<T> {
229        let x0 = neg_mlaf(self.v[0], b.v[0], c.v[0]);
230        let x1 = neg_mlaf(self.v[1], b.v[1], c.v[1]);
231        let x2 = neg_mlaf(self.v[2], b.v[2], c.v[2]);
232        Vector3 { v: [x0, x1, x2] }
233    }
234}
235
236impl<T: Copy + Mul<T, Output = T> + Add<T, Output = T> + MulAdd<T, Output = T>>
237    FusedMultiplyAdd<Vector4<T>> for Vector4<T>
238{
239    #[inline(always)]
240    fn mla(&self, b: Vector4<T>, c: Vector4<T>) -> Vector4<T> {
241        let x0 = mlaf(self.v[0], b.v[0], c.v[0]);
242        let x1 = mlaf(self.v[1], b.v[1], c.v[1]);
243        let x2 = mlaf(self.v[2], b.v[2], c.v[2]);
244        let x3 = mlaf(self.v[3], b.v[3], c.v[3]);
245        Vector4 {
246            v: [x0, x1, x2, x3],
247        }
248    }
249}
250
251impl<T: Copy + Mul<T, Output = T> + Add<T, Output = T> + MulAdd<T, Output = T> + Neg<Output = T>>
252    FusedMultiplyNegAdd<Vector4<T>> for Vector4<T>
253{
254    #[inline(always)]
255    fn neg_mla(&self, b: Vector4<T>, c: Vector4<T>) -> Vector4<T> {
256        let x0 = neg_mlaf(self.v[0], b.v[0], c.v[0]);
257        let x1 = neg_mlaf(self.v[1], b.v[1], c.v[1]);
258        let x2 = neg_mlaf(self.v[2], b.v[2], c.v[2]);
259        let x3 = neg_mlaf(self.v[3], b.v[3], c.v[3]);
260        Vector4 {
261            v: [x0, x1, x2, x3],
262        }
263    }
264}
265
266impl<T> From<T> for Vector3<T>
267where
268    T: Copy,
269{
270    fn from(value: T) -> Self {
271        Self {
272            v: [value, value, value],
273        }
274    }
275}
276
277impl<T> From<T> for Vector4<T>
278where
279    T: Copy,
280{
281    fn from(value: T) -> Self {
282        Self {
283            v: [value, value, value, value],
284        }
285    }
286}
287
288impl<T> Add<Vector3<T>> for Vector3<T>
289where
290    T: Add<Output = T> + Copy,
291{
292    type Output = Vector3<T>;
293
294    #[inline(always)]
295    fn add(self, rhs: Vector3<T>) -> Self::Output {
296        Self {
297            v: [
298                self.v[0] + rhs.v[0],
299                self.v[1] + rhs.v[1],
300                self.v[2] + rhs.v[2],
301            ],
302        }
303    }
304}
305
306impl<T> Add<Vector4<T>> for Vector4<T>
307where
308    T: Add<Output = T> + Copy,
309{
310    type Output = Vector4<T>;
311
312    #[inline(always)]
313    fn add(self, rhs: Vector4<T>) -> Self::Output {
314        Self {
315            v: [
316                self.v[0] + rhs.v[0],
317                self.v[1] + rhs.v[1],
318                self.v[2] + rhs.v[2],
319                self.v[3] + rhs.v[3],
320            ],
321        }
322    }
323}
324
325impl<T> Add<T> for Vector3<T>
326where
327    T: Add<Output = T> + Copy,
328{
329    type Output = Vector3<T>;
330
331    #[inline(always)]
332    fn add(self, rhs: T) -> Self::Output {
333        Self {
334            v: [self.v[0] + rhs, self.v[1] + rhs, self.v[2] + rhs],
335        }
336    }
337}
338
339impl<T> Add<T> for Vector4<T>
340where
341    T: Add<Output = T> + Copy,
342{
343    type Output = Vector4<T>;
344
345    #[inline(always)]
346    fn add(self, rhs: T) -> Self::Output {
347        Self {
348            v: [
349                self.v[0] + rhs,
350                self.v[1] + rhs,
351                self.v[2] + rhs,
352                self.v[3] + rhs,
353            ],
354        }
355    }
356}
357
358impl<T> Sub<Vector3<T>> for Vector3<T>
359where
360    T: Sub<Output = T> + Copy,
361{
362    type Output = Vector3<T>;
363
364    #[inline(always)]
365    fn sub(self, rhs: Vector3<T>) -> Self::Output {
366        Self {
367            v: [
368                self.v[0] - rhs.v[0],
369                self.v[1] - rhs.v[1],
370                self.v[2] - rhs.v[2],
371            ],
372        }
373    }
374}
375
376impl<T> Sub<Vector4<T>> for Vector4<T>
377where
378    T: Sub<Output = T> + Copy,
379{
380    type Output = Vector4<T>;
381
382    #[inline(always)]
383    fn sub(self, rhs: Vector4<T>) -> Self::Output {
384        Self {
385            v: [
386                self.v[0] - rhs.v[0],
387                self.v[1] - rhs.v[1],
388                self.v[2] - rhs.v[2],
389                self.v[3] - rhs.v[3],
390            ],
391        }
392    }
393}
394
395/// Matrix math helper
396#[repr(C)]
397#[derive(Copy, Clone, Debug, Default)]
398pub struct Matrix3f {
399    pub v: [[f32; 3]; 3],
400}
401
402/// Matrix math helper
403#[repr(C)]
404#[derive(Copy, Clone, Debug, Default)]
405pub struct Matrix3d {
406    pub v: [[f64; 3]; 3],
407}
408
409#[repr(C)]
410#[derive(Copy, Clone, Debug, Default)]
411pub struct Matrix3<T> {
412    pub v: [[T; 3]; 3],
413}
414
415impl<T: Copy> Matrix3<T> {
416    #[inline]
417    #[allow(dead_code)]
418    pub(crate) fn transpose(&self) -> Matrix3<T> {
419        Matrix3 {
420            v: [
421                [self.v[0][0], self.v[1][0], self.v[2][0]],
422                [self.v[0][1], self.v[1][1], self.v[2][1]],
423                [self.v[0][2], self.v[1][2], self.v[2][2]],
424            ],
425        }
426    }
427}
428
429#[repr(C)]
430#[derive(Copy, Clone, Debug, Default)]
431pub struct Matrix4f {
432    pub v: [[f32; 4]; 4],
433}
434
435pub const SRGB_MATRIX: Matrix3d = Matrix3d {
436    v: [
437        [
438            s15_fixed16_number_to_double(0x6FA2),
439            s15_fixed16_number_to_double(0x6299),
440            s15_fixed16_number_to_double(0x24A0),
441        ],
442        [
443            s15_fixed16_number_to_double(0x38F5),
444            s15_fixed16_number_to_double(0xB785),
445            s15_fixed16_number_to_double(0x0F84),
446        ],
447        [
448            s15_fixed16_number_to_double(0x0390),
449            s15_fixed16_number_to_double(0x18DA),
450            s15_fixed16_number_to_double(0xB6CF),
451        ],
452    ],
453};
454
455pub const DISPLAY_P3_MATRIX: Matrix3d = Matrix3d {
456    v: [
457        [0.515102, 0.291965, 0.157153],
458        [0.241182, 0.692236, 0.0665819],
459        [-0.00104941, 0.0418818, 0.784378],
460    ],
461};
462
463pub const BT2020_MATRIX: Matrix3d = Matrix3d {
464    v: [
465        [0.673459, 0.165661, 0.125100],
466        [0.279033, 0.675338, 0.0456288],
467        [-0.00193139, 0.0299794, 0.797162],
468    ],
469};
470
471impl Matrix4f {
472    #[inline]
473    pub fn determinant(&self) -> Option<f32> {
474        let a = self.v[0][0];
475        let b = self.v[0][1];
476        let c = self.v[0][2];
477        let d = self.v[0][3];
478
479        // Cofactor expansion
480
481        let m11 = Matrix3f {
482            v: [
483                [self.v[1][1], self.v[1][2], self.v[1][3]],
484                [self.v[2][1], self.v[2][2], self.v[2][3]],
485                [self.v[3][1], self.v[3][2], self.v[3][3]],
486            ],
487        };
488
489        let m12 = Matrix3f {
490            v: [
491                [self.v[1][0], self.v[1][2], self.v[1][3]],
492                [self.v[2][0], self.v[2][2], self.v[2][3]],
493                [self.v[3][0], self.v[3][2], self.v[3][3]],
494            ],
495        };
496
497        let m13 = Matrix3f {
498            v: [
499                [self.v[1][0], self.v[1][1], self.v[1][3]],
500                [self.v[2][0], self.v[2][1], self.v[2][3]],
501                [self.v[3][0], self.v[3][1], self.v[3][3]],
502            ],
503        };
504
505        let m14 = Matrix3f {
506            v: [
507                [self.v[1][0], self.v[1][1], self.v[1][2]],
508                [self.v[2][0], self.v[2][1], self.v[2][2]],
509                [self.v[3][0], self.v[3][1], self.v[3][2]],
510            ],
511        };
512
513        let m1_det = m11.determinant()?;
514        let m2_det = m12.determinant()?;
515        let m3_det = m13.determinant()?;
516        let m4_det = m14.determinant()?;
517
518        // Apply cofactor expansion on the first row
519        Some(a * m1_det - b * m2_det + c * m3_det - d * m4_det)
520    }
521}
522
523impl Matrix3f {
524    #[inline]
525    pub fn transpose(&self) -> Matrix3f {
526        Matrix3f {
527            v: [
528                [self.v[0][0], self.v[1][0], self.v[2][0]],
529                [self.v[0][1], self.v[1][1], self.v[2][1]],
530                [self.v[0][2], self.v[1][2], self.v[2][2]],
531            ],
532        }
533    }
534
535    pub const IDENTITY: Matrix3f = Matrix3f {
536        v: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
537    };
538
539    #[inline]
540    pub const fn test_equality(&self, other: Matrix3f) -> bool {
541        const TOLERANCE: f32 = 0.001f32;
542        let diff_r_x = (self.v[0][0] - other.v[0][0]).abs();
543        let diff_r_y = (self.v[0][1] - other.v[0][1]).abs();
544        let diff_r_z = (self.v[0][2] - other.v[0][2]).abs();
545
546        if diff_r_x > TOLERANCE || diff_r_y > TOLERANCE || diff_r_z > TOLERANCE {
547            return false;
548        }
549
550        let diff_g_x = (self.v[1][0] - other.v[1][0]).abs();
551        let diff_g_y = (self.v[1][1] - other.v[1][1]).abs();
552        let diff_g_z = (self.v[1][2] - other.v[1][2]).abs();
553
554        if diff_g_x > TOLERANCE || diff_g_y > TOLERANCE || diff_g_z > TOLERANCE {
555            return false;
556        }
557
558        let diff_b_x = (self.v[2][0] - other.v[2][0]).abs();
559        let diff_b_y = (self.v[2][1] - other.v[2][1]).abs();
560        let diff_b_z = (self.v[2][2] - other.v[2][2]).abs();
561
562        if diff_b_x > TOLERANCE || diff_b_y > TOLERANCE || diff_b_z > TOLERANCE {
563            return false;
564        }
565
566        true
567    }
568
569    #[inline]
570    pub const fn determinant(&self) -> Option<f32> {
571        let v = self.v;
572        let a0 = v[0][0] * v[1][1] * v[2][2];
573        let a1 = v[0][1] * v[1][2] * v[2][0];
574        let a2 = v[0][2] * v[1][0] * v[2][1];
575
576        let s0 = v[0][2] * v[1][1] * v[2][0];
577        let s1 = v[0][1] * v[1][0] * v[2][2];
578        let s2 = v[0][0] * v[1][2] * v[2][1];
579
580        let j = a0 + a1 + a2 - s0 - s1 - s2;
581        if j == 0. {
582            return None;
583        }
584        Some(j)
585    }
586
587    #[inline]
588    pub const fn inverse(&self) -> Self {
589        let v = self.v;
590        let det = self.determinant();
591        match det {
592            None => Matrix3f::IDENTITY,
593            Some(determinant) => {
594                let det = 1. / determinant;
595                let a = v[0][0];
596                let b = v[0][1];
597                let c = v[0][2];
598                let d = v[1][0];
599                let e = v[1][1];
600                let f = v[1][2];
601                let g = v[2][0];
602                let h = v[2][1];
603                let i = v[2][2];
604
605                Matrix3f {
606                    v: [
607                        [
608                            (e * i - f * h) * det,
609                            (c * h - b * i) * det,
610                            (b * f - c * e) * det,
611                        ],
612                        [
613                            (f * g - d * i) * det,
614                            (a * i - c * g) * det,
615                            (c * d - a * f) * det,
616                        ],
617                        [
618                            (d * h - e * g) * det,
619                            (b * g - a * h) * det,
620                            (a * e - b * d) * det,
621                        ],
622                    ],
623                }
624            }
625        }
626    }
627
628    #[inline]
629    pub fn mul_row<const R: usize>(&self, rhs: f32) -> Self {
630        if R == 0 {
631            Self {
632                v: [(Vector3f { v: self.v[0] } * rhs).v, self.v[1], self.v[2]],
633            }
634        } else if R == 1 {
635            Self {
636                v: [self.v[0], (Vector3f { v: self.v[1] } * rhs).v, self.v[2]],
637            }
638        } else if R == 2 {
639            Self {
640                v: [self.v[0], self.v[1], (Vector3f { v: self.v[2] } * rhs).v],
641            }
642        } else {
643            unimplemented!()
644        }
645    }
646
647    #[inline]
648    pub const fn mul_row_vector<const R: usize>(&self, rhs: Vector3f) -> Self {
649        if R == 0 {
650            Self {
651                v: [
652                    (Vector3f { v: self.v[0] }.const_mul_vector(rhs)).v,
653                    self.v[1],
654                    self.v[2],
655                ],
656            }
657        } else if R == 1 {
658            Self {
659                v: [
660                    self.v[0],
661                    (Vector3f { v: self.v[1] }.const_mul_vector(rhs)).v,
662                    self.v[2],
663                ],
664            }
665        } else if R == 2 {
666            Self {
667                v: [
668                    self.v[0],
669                    self.v[1],
670                    (Vector3f { v: self.v[2] }.const_mul_vector(rhs)).v,
671                ],
672            }
673        } else {
674            unimplemented!()
675        }
676    }
677
678    #[inline]
679    pub const fn mul_vector(&self, other: Vector3f) -> Vector3f {
680        let x = self.v[0][1] * other.v[1] + self.v[0][2] * other.v[2] + self.v[0][0] * other.v[0];
681        let y = self.v[1][0] * other.v[0] + self.v[1][1] * other.v[1] + self.v[1][2] * other.v[2];
682        let z = self.v[2][0] * other.v[0] + self.v[2][1] * other.v[1] + self.v[2][2] * other.v[2];
683        Vector3f { v: [x, y, z] }
684    }
685
686    /// Multiply using FMA
687    #[inline]
688    pub fn f_mul_vector(&self, other: Vector3f) -> Vector3f {
689        let x = mlaf(
690            mlaf(self.v[0][1] * other.v[1], self.v[0][2], other.v[2]),
691            self.v[0][0],
692            other.v[0],
693        );
694        let y = mlaf(
695            mlaf(self.v[1][0] * other.v[0], self.v[1][1], other.v[1]),
696            self.v[1][2],
697            other.v[2],
698        );
699        let z = mlaf(
700            mlaf(self.v[2][0] * other.v[0], self.v[2][1], other.v[1]),
701            self.v[2][2],
702            other.v[2],
703        );
704        Vector3f { v: [x, y, z] }
705    }
706
707    #[inline]
708    pub fn mat_mul(&self, other: Matrix3f) -> Self {
709        let mut result = Matrix3f::default();
710
711        for i in 0..3 {
712            for j in 0..3 {
713                result.v[i][j] = mlaf(
714                    mlaf(self.v[i][0] * other.v[0][j], self.v[i][1], other.v[1][j]),
715                    self.v[i][2],
716                    other.v[2][j],
717                );
718            }
719        }
720
721        result
722    }
723
724    #[inline]
725    pub const fn mat_mul_const(&self, other: Matrix3f) -> Self {
726        let mut result = Matrix3f { v: [[0f32; 3]; 3] };
727        let mut i = 0usize;
728        while i < 3 {
729            let mut j = 0usize;
730            while j < 3 {
731                result.v[i][j] = self.v[i][0] * other.v[0][j]
732                    + self.v[i][1] * other.v[1][j]
733                    + self.v[i][2] * other.v[2][j];
734                j += 1;
735            }
736            i += 1;
737        }
738
739        result
740    }
741
742    #[inline]
743    pub const fn to_f64(&self) -> Matrix3d {
744        Matrix3d {
745            v: [
746                [
747                    self.v[0][0] as f64,
748                    self.v[0][1] as f64,
749                    self.v[0][2] as f64,
750                ],
751                [
752                    self.v[1][0] as f64,
753                    self.v[1][1] as f64,
754                    self.v[1][2] as f64,
755                ],
756                [
757                    self.v[2][0] as f64,
758                    self.v[2][1] as f64,
759                    self.v[2][2] as f64,
760                ],
761            ],
762        }
763    }
764}
765
766impl Matrix3d {
767    #[inline]
768    pub fn transpose(&self) -> Matrix3d {
769        Matrix3d {
770            v: [
771                [self.v[0][0], self.v[1][0], self.v[2][0]],
772                [self.v[0][1], self.v[1][1], self.v[2][1]],
773                [self.v[0][2], self.v[1][2], self.v[2][2]],
774            ],
775        }
776    }
777
778    pub const IDENTITY: Matrix3d = Matrix3d {
779        v: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
780    };
781
782    #[inline]
783    pub const fn test_equality(&self, other: Matrix3d) -> bool {
784        const TOLERANCE: f64 = 0.001f64;
785        let diff_r_x = (self.v[0][0] - other.v[0][0]).abs();
786        let diff_r_y = (self.v[0][1] - other.v[0][1]).abs();
787        let diff_r_z = (self.v[0][2] - other.v[0][2]).abs();
788
789        if diff_r_x > TOLERANCE || diff_r_y > TOLERANCE || diff_r_z > TOLERANCE {
790            return false;
791        }
792
793        let diff_g_x = (self.v[1][0] - other.v[1][0]).abs();
794        let diff_g_y = (self.v[1][1] - other.v[1][1]).abs();
795        let diff_g_z = (self.v[1][2] - other.v[1][2]).abs();
796
797        if diff_g_x > TOLERANCE || diff_g_y > TOLERANCE || diff_g_z > TOLERANCE {
798            return false;
799        }
800
801        let diff_b_x = (self.v[2][0] - other.v[2][0]).abs();
802        let diff_b_y = (self.v[2][1] - other.v[2][1]).abs();
803        let diff_b_z = (self.v[2][2] - other.v[2][2]).abs();
804
805        if diff_b_x > TOLERANCE || diff_b_y > TOLERANCE || diff_b_z > TOLERANCE {
806            return false;
807        }
808
809        true
810    }
811
812    #[inline]
813    pub const fn determinant(&self) -> Option<f64> {
814        let v = self.v;
815        let a0 = v[0][0] * v[1][1] * v[2][2];
816        let a1 = v[0][1] * v[1][2] * v[2][0];
817        let a2 = v[0][2] * v[1][0] * v[2][1];
818
819        let s0 = v[0][2] * v[1][1] * v[2][0];
820        let s1 = v[0][1] * v[1][0] * v[2][2];
821        let s2 = v[0][0] * v[1][2] * v[2][1];
822
823        let j = a0 + a1 + a2 - s0 - s1 - s2;
824        if j == 0. {
825            return None;
826        }
827        Some(j)
828    }
829
830    #[inline]
831    pub const fn inverse(&self) -> Self {
832        let v = self.v;
833        let det = self.determinant();
834        match det {
835            None => Matrix3d::IDENTITY,
836            Some(determinant) => {
837                let det = 1. / determinant;
838                let a = v[0][0];
839                let b = v[0][1];
840                let c = v[0][2];
841                let d = v[1][0];
842                let e = v[1][1];
843                let f = v[1][2];
844                let g = v[2][0];
845                let h = v[2][1];
846                let i = v[2][2];
847
848                Matrix3d {
849                    v: [
850                        [
851                            (e * i - f * h) * det,
852                            (c * h - b * i) * det,
853                            (b * f - c * e) * det,
854                        ],
855                        [
856                            (f * g - d * i) * det,
857                            (a * i - c * g) * det,
858                            (c * d - a * f) * det,
859                        ],
860                        [
861                            (d * h - e * g) * det,
862                            (b * g - a * h) * det,
863                            (a * e - b * d) * det,
864                        ],
865                    ],
866                }
867            }
868        }
869    }
870
871    #[inline]
872    pub fn mul_row<const R: usize>(&self, rhs: f64) -> Self {
873        if R == 0 {
874            Self {
875                v: [(Vector3d { v: self.v[0] } * rhs).v, self.v[1], self.v[2]],
876            }
877        } else if R == 1 {
878            Self {
879                v: [self.v[0], (Vector3d { v: self.v[1] } * rhs).v, self.v[2]],
880            }
881        } else if R == 2 {
882            Self {
883                v: [self.v[0], self.v[1], (Vector3d { v: self.v[2] } * rhs).v],
884            }
885        } else {
886            unimplemented!()
887        }
888    }
889
890    #[inline]
891    pub const fn mul_row_vector<const R: usize>(&self, rhs: Vector3d) -> Self {
892        if R == 0 {
893            Self {
894                v: [
895                    (Vector3d { v: self.v[0] }.const_mul_vector(rhs)).v,
896                    self.v[1],
897                    self.v[2],
898                ],
899            }
900        } else if R == 1 {
901            Self {
902                v: [
903                    self.v[0],
904                    (Vector3d { v: self.v[1] }.const_mul_vector(rhs)).v,
905                    self.v[2],
906                ],
907            }
908        } else if R == 2 {
909            Self {
910                v: [
911                    self.v[0],
912                    self.v[1],
913                    (Vector3d { v: self.v[2] }.const_mul_vector(rhs)).v,
914                ],
915            }
916        } else {
917            unimplemented!()
918        }
919    }
920
921    #[inline]
922    pub const fn mul_vector(&self, other: Vector3d) -> Vector3d {
923        let x = self.v[0][1] * other.v[1] + self.v[0][2] * other.v[2] + self.v[0][0] * other.v[0];
924        let y = self.v[1][0] * other.v[0] + self.v[1][1] * other.v[1] + self.v[1][2] * other.v[2];
925        let z = self.v[2][0] * other.v[0] + self.v[2][1] * other.v[1] + self.v[2][2] * other.v[2];
926        Vector3::<f64> { v: [x, y, z] }
927    }
928
929    #[inline]
930    pub fn mat_mul(&self, other: Matrix3d) -> Self {
931        let mut result = Matrix3d::default();
932
933        for i in 0..3 {
934            for j in 0..3 {
935                result.v[i][j] = mlaf(
936                    mlaf(self.v[i][0] * other.v[0][j], self.v[i][1], other.v[1][j]),
937                    self.v[i][2],
938                    other.v[2][j],
939                );
940            }
941        }
942
943        result
944    }
945
946    #[inline]
947    pub const fn mat_mul_const(&self, other: Matrix3d) -> Self {
948        let mut result = Matrix3d { v: [[0.; 3]; 3] };
949        let mut i = 0usize;
950        while i < 3 {
951            let mut j = 0usize;
952            while j < 3 {
953                result.v[i][j] = self.v[i][0] * other.v[0][j]
954                    + self.v[i][1] * other.v[1][j]
955                    + self.v[i][2] * other.v[2][j];
956                j += 1;
957            }
958            i += 1;
959        }
960
961        result
962    }
963
964    #[inline]
965    pub const fn to_f32(&self) -> Matrix3f {
966        Matrix3f {
967            v: [
968                [
969                    self.v[0][0] as f32,
970                    self.v[0][1] as f32,
971                    self.v[0][2] as f32,
972                ],
973                [
974                    self.v[1][0] as f32,
975                    self.v[1][1] as f32,
976                    self.v[1][2] as f32,
977                ],
978                [
979                    self.v[2][0] as f32,
980                    self.v[2][1] as f32,
981                    self.v[2][2] as f32,
982                ],
983            ],
984        }
985    }
986}
987
988impl Mul<Matrix3f> for Matrix3f {
989    type Output = Matrix3f;
990
991    #[inline]
992    fn mul(self, rhs: Matrix3f) -> Self::Output {
993        self.mat_mul(rhs)
994    }
995}
996
997impl Mul<Matrix3d> for Matrix3d {
998    type Output = Matrix3d;
999
1000    #[inline]
1001    fn mul(self, rhs: Matrix3d) -> Self::Output {
1002        self.mat_mul(rhs)
1003    }
1004}
1005
1006/// Holds CIE XYZ representation
1007#[repr(C)]
1008#[derive(Clone, Debug, Copy, Default)]
1009pub struct Xyz {
1010    pub x: f32,
1011    pub y: f32,
1012    pub z: f32,
1013}
1014
1015impl Xyz {
1016    #[inline]
1017    pub fn to_xyy(&self) -> [f32; 3] {
1018        let sums = self.x + self.y + self.z;
1019        if sums == 0. {
1020            return [0., 0., self.y];
1021        }
1022        let x = self.x / sums;
1023        let y = self.y / sums;
1024        let yb = self.y;
1025        [x, y, yb]
1026    }
1027
1028    #[inline]
1029    pub fn from_xyy(xyy: [f32; 3]) -> Xyz {
1030        let reciprocal = if xyy[1] != 0. {
1031            1. / xyy[1] * xyy[2]
1032        } else {
1033            0.
1034        };
1035        let x = xyy[0] * reciprocal;
1036        let y = xyy[2];
1037        let z = (1. - xyy[0] - xyy[1]) * reciprocal;
1038        Xyz { x, y, z }
1039    }
1040}
1041
1042/// Holds CIE XYZ representation, in double precision
1043#[repr(C)]
1044#[derive(Clone, Debug, Copy, Default)]
1045pub struct Xyzd {
1046    pub x: f64,
1047    pub y: f64,
1048    pub z: f64,
1049}
1050
1051macro_rules! define_xyz {
1052    ($xyz_name:ident, $im_type: ident, $matrix: ident) => {
1053        impl PartialEq<Self> for $xyz_name {
1054            #[inline]
1055            fn eq(&self, other: &Self) -> bool {
1056                const TOLERANCE: $im_type = 0.0001;
1057                let dx = (self.x - other.x).abs();
1058                let dy = (self.y - other.y).abs();
1059                let dz = (self.z - other.z).abs();
1060                dx < TOLERANCE && dy < TOLERANCE && dz < TOLERANCE
1061            }
1062        }
1063
1064        impl $xyz_name {
1065            #[inline]
1066            pub const fn new(x: $im_type, y: $im_type, z: $im_type) -> Self {
1067                Self { x, y, z }
1068            }
1069
1070            #[inline]
1071            pub const fn to_vector(self) -> Vector3f {
1072                Vector3f {
1073                    v: [self.x as f32, self.y as f32, self.z as f32],
1074                }
1075            }
1076
1077            #[inline]
1078            pub const fn to_vector_d(self) -> Vector3d {
1079                Vector3d {
1080                    v: [self.x as f64, self.y as f64, self.z as f64],
1081                }
1082            }
1083
1084            #[inline]
1085            pub fn matrix_mul(&self, matrix: $matrix) -> Self {
1086                let x = mlaf(
1087                    mlaf(self.x * matrix.v[0][0], self.y, matrix.v[0][1]),
1088                    self.z,
1089                    matrix.v[0][2],
1090                );
1091                let y = mlaf(
1092                    mlaf(self.x * matrix.v[1][0], self.y, matrix.v[1][1]),
1093                    self.z,
1094                    matrix.v[1][2],
1095                );
1096                let z = mlaf(
1097                    mlaf(self.x * matrix.v[2][0], self.y, matrix.v[2][1]),
1098                    self.z,
1099                    matrix.v[2][2],
1100                );
1101                Self::new(x, y, z)
1102            }
1103
1104            #[inline]
1105            pub fn from_linear_rgb(rgb: crate::Rgb<$im_type>, rgb_to_xyz: $matrix) -> Self {
1106                let r = rgb.r;
1107                let g = rgb.g;
1108                let b = rgb.b;
1109
1110                let transform = rgb_to_xyz;
1111
1112                let new_r = mlaf(
1113                    mlaf(r * transform.v[0][0], g, transform.v[0][1]),
1114                    b,
1115                    transform.v[0][2],
1116                );
1117
1118                let new_g = mlaf(
1119                    mlaf(r * transform.v[1][0], g, transform.v[1][1]),
1120                    b,
1121                    transform.v[1][2],
1122                );
1123
1124                let new_b = mlaf(
1125                    mlaf(r * transform.v[2][0], g, transform.v[2][1]),
1126                    b,
1127                    transform.v[2][2],
1128                );
1129
1130                $xyz_name::new(new_r, new_g, new_b)
1131            }
1132
1133            #[inline]
1134            pub fn normalize(self) -> Self {
1135                if self.y == 0. {
1136                    return Self {
1137                        x: 0.,
1138                        y: 1.0,
1139                        z: 0.0,
1140                    };
1141                }
1142                let reciprocal = 1. / self.y;
1143                Self {
1144                    x: self.x * reciprocal,
1145                    y: 1.0,
1146                    z: self.z * reciprocal,
1147                }
1148            }
1149
1150            #[inline]
1151            pub fn to_linear_rgb(self, rgb_to_xyz: $matrix) -> crate::Rgb<$im_type> {
1152                let x = self.x;
1153                let y = self.y;
1154                let z = self.z;
1155
1156                let transform = rgb_to_xyz;
1157
1158                let new_r = mlaf(
1159                    mlaf(x * transform.v[0][0], y, transform.v[0][1]),
1160                    z,
1161                    transform.v[0][2],
1162                );
1163
1164                let new_g = mlaf(
1165                    mlaf(x * transform.v[1][0], y, transform.v[1][1]),
1166                    z,
1167                    transform.v[1][2],
1168                );
1169
1170                let new_b = mlaf(
1171                    mlaf(x * transform.v[2][0], y, transform.v[2][1]),
1172                    z,
1173                    transform.v[2][2],
1174                );
1175
1176                crate::Rgb::<$im_type>::new(new_r, new_g, new_b)
1177            }
1178        }
1179
1180        impl Mul<$im_type> for $xyz_name {
1181            type Output = $xyz_name;
1182
1183            #[inline]
1184            fn mul(self, rhs: $im_type) -> Self::Output {
1185                Self {
1186                    x: self.x * rhs,
1187                    y: self.y * rhs,
1188                    z: self.z * rhs,
1189                }
1190            }
1191        }
1192
1193        impl Mul<$matrix> for $xyz_name {
1194            type Output = $xyz_name;
1195
1196            #[inline]
1197            fn mul(self, rhs: $matrix) -> Self::Output {
1198                self.matrix_mul(rhs)
1199            }
1200        }
1201
1202        impl Mul<$xyz_name> for $xyz_name {
1203            type Output = $xyz_name;
1204
1205            #[inline]
1206            fn mul(self, rhs: $xyz_name) -> Self::Output {
1207                Self {
1208                    x: self.x * rhs.x,
1209                    y: self.y * rhs.y,
1210                    z: self.z * rhs.z,
1211                }
1212            }
1213        }
1214
1215        impl Div<$xyz_name> for $xyz_name {
1216            type Output = $xyz_name;
1217
1218            #[inline]
1219            fn div(self, rhs: $xyz_name) -> Self::Output {
1220                Self {
1221                    x: self.x / rhs.x,
1222                    y: self.y / rhs.y,
1223                    z: self.z / rhs.z,
1224                }
1225            }
1226        }
1227
1228        impl Div<$im_type> for $xyz_name {
1229            type Output = $xyz_name;
1230
1231            #[inline]
1232            fn div(self, rhs: $im_type) -> Self::Output {
1233                Self {
1234                    x: self.x / rhs,
1235                    y: self.y / rhs,
1236                    z: self.z / rhs,
1237                }
1238            }
1239        }
1240    };
1241}
1242
1243impl Xyz {
1244    pub fn to_xyzd(self) -> Xyzd {
1245        Xyzd {
1246            x: self.x as f64,
1247            y: self.y as f64,
1248            z: self.z as f64,
1249        }
1250    }
1251}
1252
1253impl Xyzd {
1254    pub fn to_xyz(self) -> Xyz {
1255        Xyz {
1256            x: self.x as f32,
1257            y: self.y as f32,
1258            z: self.z as f32,
1259        }
1260    }
1261
1262    pub fn to_xyzd(self) -> Xyzd {
1263        Xyzd {
1264            x: self.x,
1265            y: self.y,
1266            z: self.z,
1267        }
1268    }
1269}
1270
1271define_xyz!(Xyz, f32, Matrix3f);
1272define_xyz!(Xyzd, f64, Matrix3d);
moxcms/matrix.rs

moxcms/
matrix.rs
